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Abstract 

A  method  is  presented  to  couple  and  solve  the  optimal  control  and  the  optimal  es¬ 
timation  problems  simultaneously,  allowing  systems  with  bearing-only  sensors  to  ma¬ 
neuver  to  obtain  observability  for  relative  navigation  without  unnecessarily  detracting 
from  a  primary  mission.  A  fundamentally  new  approach  to  trajectory  optimization 
and  the  dual  control  problem  is  presented,  constraining  polynomial  approximations  of 
the  Fisher  Information  Matrix  to  provide  an  information  gradient  and  allow  prescrip¬ 
tion  of  the  level  of  future  estimation  certainty  required  for  mission  accomplishment. 

Disturbances,  modeling  deficiencies,  and  corrupted  measurements  are  addressed 
recursively  using  Radau  pseudospectral  collocation  methods  and  sequential  quadratic 
programming  for  the  optimal  path  and  an  Unscented  Kalman  Filter  for  the  target 
position  estimate.  The  underlying  real-time  optimal  control  (RTOC)  algorithm  is 
developed,  specifically  addressing  limitations  of  current  techniques  that  lose  error 
integration. 

The  resulting  guidance  method  can  be  applied  to  any  bearing-only  system,  such 
as  submarines  using  passive  sonar,  anti-radiation  missiles,  or  small  UAVs  seeking 
to  land  on  power  lines  for  energy  harvesting.  System  integration,  variable  timing 
methods,  and  discontinuity  management  techniques  are  provided  for  actual  hardware 
implementation.  Validation  is  accomplished  with  both  simulation  and  flight  test, 
autonomously  landing  a  quadrotor  helicopter  on  a  wire. 
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STOCHASTIC  REAL-TIME  OPTIMAL  CONTROL:  A  PSEUDOSPECTRAL 
APPROACH  FOR  BEARING-ONLY  TRAJECTORY  OPTIMIZATION 

I.  Introduction 


“The  difficulty  is  designing  machines  that  can  approximate  the  remark¬ 
able  human  ability  to  reason  and  make  decisions  in  an  environment  of 
uncertainty  and  imprecision.”  -Loth  A.  Zadeh  [120] 


/  I  his  dissertation  addresses  a  problem  at  the  crossroads  of  the  fields  of  esti- 
X-  mation  and  optimal  control.  For  a  basic  two-point  boundary  value  problem 
(TPBVP),  optimal  control  can  be  thought  of  most  simply  as  finding  the  “best”  path 
and  control  to  get  from  “here”  to  “there.”  On  the  navigation  side,  optimal  estimation 
can  be  thought  of  as  finding  the  best  guess  of  a  target  location  given  a  set  of  imperfect 
measurements.  Present  levels  of  technology  are  excellent  at  doing  both.  . .  individually. 
But  what  is  the  best  path  to  get  to  a  target  with  a  location  that  is  not  well  known?  If 
the  quality  of  the  target  estimate  can  be  improved  by  varying  the  path  taken,  what  is 
the  optimal  path  that  will  accomplish  a  primary  mission,  while  maneuvering  enough 
to  get  the  estimation  quality  required  for  success  in  that  mission?  This  research 
seeks  an  automated  method  to  find  that  solution  quickly,  fast  enough  for  real-time 
guidance,  and  robust  enough  for  the  uncertainties  and  disturbances  of  real  life. 

The  human  mind  is  an  amazing  optimization  machine  that  solves  these  problems 
regularly.  Every  control  decision,  from  the  way  you  drive  home  from  work  to  the  way 
you  hit  a  baseball,  is  made  in  an  optimal  manner.  We  continually  try  to  maximize  or 
minimize  some  performance  index  of  time,  effort,  power,  accuracy,  or  a  myriad  of  other 
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considerations — often  simultaneously.  In  the  world  of  control,  a  modern  computer  can 
accomplish  a  task  far  more  precisely,  but  cannot  compare  in  ability  to  deal  with  a 
wide  range  of  uncertain  inputs  and  incomplete  information.  In  the  estimation  realm, 
we  have  finely-honed  computer  filtering  algorithms  and  data  processing  techniques 
to  “squeeze  out”  every  piece  of  useful  sensor  information  provided,  but  our  machines 
lack  the  human’s  intuitive  feel  for  how  to  move  to  make  that  information  better.  The 
ability  to  sense  what  we  are  missing — and  how  to  get  it — causes  us  to  tilt  our  ear,  to 
lean  around  a  corner,  and  to  slow  down  before  a  blind  intersection.  Improving  the 
information  isn’t  the  primary  mission,  but  it  is  done  “enough”  to  meet  the  needs  of 
a  higher  goal. 

The  goal  of  this  research  is  to  provide  this  capability  to  an  autonomous  controller, 
capable  of  being  used  in  real-time,  with  the  recognition  that  what  is  optimal  in  a 
stochastic  environment  is  not  only  a  function  of  “What  do  I  want?”  but  also  of 
“What  do  I  know  now,  and  how  well  do  I  know  it?”  A  guidance  system  should  be 
able  to  figure  out  what  information  is  still  needed  for  success,  and  be  able  to  produce 
the  path  and  control  to  get  it — taking  as  little  as  possible  away  from  the  primary 
mission. 


1.1  Motivation 

The  ability  to  maneuver  in  relation  to  current  levels  of  target  knowledge  will 
directly  benefit  systems  in  which  estimation  performance  is  dependent  upon  the  ge¬ 
ometry  of  the  constellation  of  measurements  that  has  been  received.  This  research 
focuses  on  path  guidance  to  land  small  aircraft  on  power  lines  using  a  single  camera 
for  a  sensor.  Range  to  the  target  must  be  found  through  maneuver,  as  is  the  case  for 
several  examples  of  bearing-only  systems  that  would  benefit  from  the  same  type  of 
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guidance,  such  as  submarines  using  passive  sonar,  high-speed  anti-radiation  missiles 
(HARM),  or  systems  with  infrared  search  and  tracking  systems  (IRSTS),  shown  in 


Figure  [T} 


(a)  Seawolf  Submarine  (Photo: 
U.S.  Navy) 


(c)  HARM  Missile  (Photo: 
FAS.org) 


(b)  IRSTS  (Photo:  MILAVIA) 


(d)  Aeryon  Scout  Quadrotor 
(Photo:  Aeryon  Labs) 


Figure  1.  Bearing-only  Systems 

Each  of  these  systems  relies  on  a  bearing-sensor  to  track  targets  as  part  of  a 
greater  mission.  The  HARM  system  receives  bearing  information  from  the  electronic 
emissions  of  a  ground  radar  site  and  must  determine  a  path  to  hit  the  site  with 
maximum  energy  while  respecting  its  own  sensor  limitations  j2H]-  Motion  away  from 
the  target  line  of  sight  (LOS)  increases  the  fidelity  of  the  target  position  estimate, 
but  can  simultaneously  decrease  the  missile’s  energy. 

Submarines  (and  other  Naval  vessels)  do  almost  all  of  their  target  motion  analysis 
passively  [33] ,  with  bearing-only  sonar  tracking  algorithms  very  similar  to  the  optical 
tracking  problem  of  the  IRSTS.  In  both  cases,  choosing  to  use  active  ranging  (via 
sonar  or  radar,  respectively),  while  much  more  accurate,  gives  away  the  presence 
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(and  interest)  of  the  sensor,  and  its  position.  Passive  ranging  requires  maneuver,  and 
current  submarine  techniques  to  accomplish  it  haven’t  significantly  changed  since  the 
1950s — a  turn  is  made  orthogonal  to  the  target  LOS  and  that  heading  is  held  long 
enough  to  produce  a  bearing  fan  of  measurements  suitable  for  algorithms  such  as 
Ekelund  or  Spiess  ranging  |jlf)4j.  followed  by  one  extra  turn  to  eliminate  ambiguities. 
A  maneuver  that  would  do  this  while  closing  to  attack  range,  or  while  increasing 
standoff  distance  is  left  to  the  “seat  of  the  pants”  intuition  of  the  commander  [43]. 
Ideally,  an  automated  guidance  system  could  integrate  the  tools  of  optimal  control 
and  bearing-only  target  analysis  to  maneuver  the  submarine  in  such  a  manner  that 
it  achieves  exactly  the  minimum  target  certainty  required  for  the  fire-control  system 
precisely  at  the  time  the  submarine  reaches  maximum  torpedo  range. 

1.1.1  Bearing-only  Target  Analysis. 

Bearing-only  target  analysis  is  a  classic  estimation  problem,  and  exists  in  appli¬ 
cations  from  basic  triangulation  in  land  surveying  to  missile  detection  systems  [42], 
The  inability  to  sense  range  with  each  measurement,  combined  with  the  inherent  non¬ 
linearity  of  the  problem,  make  estimation  of  a  target  location,  or  source,  problematic. 
One  common  solution  is  to  take  measurements  from  non-collocated  sensors,  as  is  done 
with  stereo  vision,  in  Figure  [2] 
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The  limitation  to  this  technique  is  that  the  size  and  shape  of  the  uncertainty 
“bubble”  around  the  estimate  is  a  function  of  the  baseline  between  the  sensors.  The 
farther  the  target,  the  more  baseline  is  required  for  resolution.  Systems  such  as  small 
aircraft  with  optical  cameras  and  fighter  jets  with  IRSTS  lack  the  physical  dimensions 
for  enough  baseline  to  make  stereo  vision  effective  at  their  respective  ranges  of  interest. 

For  a  monocular  system,  range  estimation  is  analogous,  but  the  sensor  must  phys¬ 
ically  be  moved  orthogonal  to  the  target  LOS  (or  be  in  a  position  to  observe  orthogo¬ 
nal  target  motion),  artificially  creating  enough  baseline  to  enable  triangulation.  This 
motion  comes  at  the  expense  of  the  primary  mission,  unless  the  entire  purpose  is 
localization  of  the  target.  Depending  on  the  accuracy  of  the  sensor,  a  wide  range  of 
aspect  angles  may  be  required  for  a  reasonable  range  estimate.  If  other,  more  accu¬ 
rate,  ranging  sensors  are  available,  such  as  laser  range  finders,  radar,  or  active  sonar, 
these  would  obviously  be  preferred.  However,  many  systems  are  limited  by  stealth 
considerations,  physical  dimensions,  or  payload  capacity  to  a  single,  passive  bearing 
sensor.  One  such  system  is  DARPA’s  19  gram  Nano-Hummingbird  with  a  monocular 
camera  shown  in  Figure  [3} 


Figure  3.  DARPA  Nano-Hummingbird  (Photo:  AeroVironment) 
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1.1.2  Power  Harvesting. 


The  Department  of  Defense  (DoD)  has  dedicated  an  unprecedented  amount  of  time 
and  energy  into  research  of  small,  unmanned  aerial  systems  (sUAS)  in  recent  years 
for  a  variety  of  purposes  [M|.  The  ability  to  move  a  sensor,  or  other  small  payload, 
to  a  particular  site  for  surveillance  and  other  purposes  provides  great  capabilities, 
particularly  if  it  can  be  done  undetected  S5-  The  trend  of  recent  design  has  been 
to  reduce  the  size  of  these  vehicles  dramatically.  The  Nano- Hummingbird  is  a  great 
example,  but  current  tactical  systems  are  on  the  order  of  the  Wasp  and  the  Raven®  B, 
with  approximate  wingspans  of  72-cm  and  140-cm,  respectively,  shown  in  Figure  |4j 


(b)  Raven®  B 

Figure  4.  Current  Tactical  sUAS  Systems  with  Monocular  Sensors  (AeroVironment) 

Obviously,  sensor  quality  and  availability  decrease  commensurate  with  the  ve¬ 
hicle’s  size  and  weight.  In  addition,  smaller  systems  have  lower  flight  speeds  and 
greatly  decreased  range.  Compounding  the  problem  is  the  obvious  lack  of  payload 
capacity.  For  electric  motors,  battery  life  is  severely  limited  by  allowable  payload. 
This  translates  into  short  range  assets  that  have  limited  persistence. 

One  possible  method  of  significantly  extending  both  range  and  station  time  is 
energy  harvesting  off  of  available  power  lines  during  a  mission  na.  The  Power  Line 
Urban  Sentry  (PLUS)  program  at  the  Air  Force  Research  Laboratory  (AFRL)  with 
the  work  of  Defense  Research  Associates  (DRA)  has  been  successful  with  recharging 
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batteries  through  induction,  by  clamping  around  medium-sized  power  lines,  such  as 
you  would  see  in  a  typical  neighborhood  [98]. 

This  technique  has  powered  observation  sensors  with  a  camera,  modem,  and  server 
board,  allowing  the  camera  to  be  accessed  and  controlled  by  a  common  iPhone.  The 
concept  is  to  extend  this  technology  to  sUASs,  hanging  them  from  a  power  line  until 
recharged  as  in  Figure  [5] 


(a)  Camera  Powered  by  Passive  Induction 
(Photo:  DRA) 


Figure  5.  Power  Line  Harvesting 

The  observation  camera  technology  is  at  the  early  heldable  stages,  but  currently 
the  weight  of  an  inductor  clamp  large  enough  to  recharge  a  sUAS  sized  battery  in  a 
reasonable  time  is  problematic,  given  the  extremely  limited  available  payload  capac¬ 
ity  of  small  aircraft.  In  addition  to  battery  development,  future  advances  in  inductive 
technology,  such  as  recharging  pads  for  cell  phones,  will  almost  certainly  open  har¬ 
vesting  as  a  viable  future  option  for  power  regeneration.  Even  now,  the  technology 
exists  to  design  a  “home  base”  power  station,  attached  the  same  way  as  the  current 
sensor  suites.  A  sUAS  could  be  used  locally  from  the  position  of  the  base,  such  as 
flying  preprogrammed  loops  for  border  security,  etc.,  and  could  return  to  the  base  for 
power  replenishment.  Multiple  vehicles  could  cycle  off  of  the  power  line  for  continuous 
coverage. 
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There  is  a  near-term  requirement,  therefore,  for  a  control  algorithm  to  find,  ap¬ 
proach,  and  perch  on  power  lines.  The  critical  difficulty  in  this  is  the  measurement 
of  the  relative  position  between  the  sUAS  and  the  power  line  itself.  AFRL’s  research 
has  shown  that  avoiding  the  issue  by  merely  tracking  the  angle  to  the  power  line 
and  running  into  it  at  flight  speed  (with  a  hook  system  designed  for  that  purpose)  is 
overly  abrupt  and  can  lead  to  failure  of  the  vehicle  m.  Morphing  of  the  wings  in 
an  attempt  to  decrease  the  stall  speed  has  been  attempted  }116j,  but  the  thought  of 
automating  this  process  only  accentuates  the  great  need  for  accurate  relative  position 
data  between  the  sUAS  and  the  intended  landing  point.  As  this  research  is  extended 
from  perching  on  power  lines  to  rooftops  and  window  ledges,  the  price  for  a  “miss” 
goes  up,  and  the  requirement  for  accurate  relative  data  becomes  even  greater.  Using 
preset  landing  coordinates  is  ineffective  and  removes  too  much  flexibility  from  the 
system.  Though  we  have  made  great  strides  in  GPS  receiver  miniaturization  and  ac¬ 
curacy,  mensurated  coordinates  of  every  power  line  out  there  are  simply  not  available. 
In  theory,  we  could  use  space-based  assets  and  extensive  mission  planning  to  get  an 
exact  point  to  fly  to,  but  experience  from  attempts  at  open-loop  control  for  relative 
taskings  strongly  suggests  that  this  is  not  a  feasible  solution.  Real-time  feedback 
of  the  relative  position  must  be  made  available,  and  in  the  bearing-only  sensor  case 
where  range  is  important,  this  must  be  attained  through  manipulation  of  the  path. 


1.2  Important  Semantics 

Throughout  the  document,  the  following  notation  and  definitions  are  used: 

•  The  Air  Force  is  making  an  effort  to  move  toward  the  use  of  the  acronyms  sUAS 
and  RPV  (Remotely  Piloted  Vehicle)  and  away  from  the  more  familiar  general 
term  UAV  (Unmanned  Aerial  Vehicle).  Since  the  power  line  scenario  is  specific 


to  the  sUAS,  that  term  will  be  used,  with  the  understanding  that  the  algorithm 
itself  could  be  applied  to  any  UAV,  obviously  on  a  different  scale  and  with  a 
different  final  mission. 

•  The  terms  path  and  trajectory  are  synonymous. 

•  The  term  observer  is  used  to  describe  a  system  that  plans  and  tracks  a  tra¬ 
jectory  to  produce  sufficient  observability  of  a  target  location  to  accomplish  a 
mission.  In  the  power  line  landing  context,  the  term  observer  will  be  used  inter¬ 
changeably  with  the  term  “vehicle,”  and  refers  to  the  entire  unmanned  system, 
including  the  sUAS  platform  and  the  associated  sensors. 

•  The  target,  or  source,  is  the  object  whose  location  the  observer  is  attempting 
to  estimate.  Note  that  the  target  is  not  necessarily  the  maneuvering  goal  of  the 
observer  (i.e.  the  submarine  needs  to  ascertain  the  position  of  a  target  contact, 
but  is  maneuvering  to  an  attack  position  relative  to  it,  not  to  the  target  itself). 
For  the  landing  scenario,  the  term  target  is  synonymous  with  power  line  (for 
the  true  application)  or  wire  (for  the  flight  test). 

•  The  approach  point  is  the  desired  maneuver  end  point  of  the  observer  during 
the  segment  of  the  mission  directed  by  the  path  planner,  which  is  the  algorithm 
that  determines  the  optimal  trajectory  for  the  given  conditions.  The  prescribed 
level  of  certainty  in  the  target  position  must  be  attained  prior  to  reaching  the 
approach  point. 

•  The  term  localization  will  be  used  to  denote  the  specific  case  where  bearing- 
only  tracking  techniques  are  applied  to  a  target  known  to  be  static. 

•  Discrete  time  steps  of  a  state  x(t  =  tk)  are  abbreviated  Xk  where  it  will  not 
cause  confusion.  Context  must  be  used  to  determine  whether  the  length  of 
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the  step  is  At  (for  the  control  station  and  autopilot),  A tmeas  (for  measurement 
updates  (3k,  Pk+i),  or  A tcaic  for  epochs.  The  term  epoch  is  reserved  for  one 
step  of  the  path  planner,  which  produces  a  complete  time  history  of  the  states 
and  controls  in  each  solution.  For  instance,  the  phrase  x0fc+1  =  x*.(t  +  A tcaic) 
means  that  the  initial  state  to  be  used  in  the  path  planner  at  epoch  k  +  1  is 
determined  by  propagating  the  state  time  history  received  at  epoch  k  forward 
to  time  t  +  AtCaic-  Values  at  the  same  time  step,  but  calculated  before  and  after 
a  measurement  update  has  been  incorporated  are  delineated  by  x((,  x%. 


1.3  Assumptions 

This  project  assumes  the  existence  of  an  observer  vehicle  with  an  indigenous  nav¬ 
igation  capability  from  a  system  such  as  GPS,  INS,  or  some  sort  of  image  processing 
such  as  optical  flow  j5U,  112].  sufficient  to  determine  its  inertial  position  relative  to 
constraining  borders,  be  they  terrain,  walls,  an  altitude  ceiling,  political  boundaries, 
etc.  The  borders  will  be  observed  as  position  limitations,  but  the  observer  is  assumed 
to  have  control  authority  to  move  freely  within  the  borders,  respecting  velocity  and 
acceleration  constraints  (obstacle  avoidance  is  not  considered).  The  observer  has 
available  processing  power  for  estimation  and  real-time  optimization. 

The  observer  is  also  assumed  to  be  equipped  with  a  bearing  sensor  capable  of 
identifying  the  target  and  producing  an  angular  measurement  to  it,  and  the  target 
is  assumed  to  be  initially  within  the  sensor’s  field-of-view  (FOV).  The  measurements 
are  delayed,  but  time  tagged,  and  corrupted  by  uncertainties  in  the  sensor  and  the 
vehicle  orientation.  Specifically  for  the  power  line  scenario,  it  is  assumed  that  the 
vertical  angle  to  the  power  line  can  be  found  with  a  line  detection  algorithm  operating 
on  sequential  images  from  an  optical  sensor.  It  is  assumed  that  the  power  line  is 
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horizontal,  and  that  the  angle  along  the  wire  is  not  observable.  Future  research  could 
certainly  expand  on  this  with  allowance  for  utility  pole  identification,  stadiametric 
ranging,  sag  analysis,  or  other  considerations.  With  no  observable  lateral  changes, 
there  is  no  benefit  to  flight  parallel  to  the  wire,  and  the  optimal  trajectory  becomes 
planar,  maneuvering  in  the  vertical  to  increase  observability.  The  submarine  variant 
of  the  problem  can  also  be  considered  planar,  only  horizontal.  In  this  case,  there 
is  some  observability  that  could  be  gained  from  vertical  motion,  but  the  realizable 
benefit  from  the  restricted  ability  to  maneuver  in  the  vertical  is  small  at  the  long 
horizontal  distances  typical  for  submarine  contacts  that  are  still  un-ranged. 


1.4  Project  Summary 

This  dissertation  proposes  a  new  method  of  approaching  the  bearing-only  trajec¬ 
tory  planning  problem  that  enables  simultaneous  consideration  of  the  optimal  control 
problem  and  prescribed  final  estimation  requirements,  overcoming  the  typical  limita¬ 
tions  of  previous  approaches.  The  trajectory  planning  goal  is  to  provide  an  optimal 
path  and  control  for  arrival  at  a  point,  or  set  of  points,  offset  relative  to  a  target  po¬ 
sition,  the  location  of  which  must  be  determined  to  a  predefined  certainty  by  varying 
the  engagement  geometry  while  receiving  stochastic,  delayed  angle  measurements.  In 
addition  to  allowing  a  general  cost  function,  the  method  treats  required  final  direc¬ 
tional  covariance  in  the  target  estimate  as  a  constraint,  optimally  considering  the 
observability  requirements  of  the  bearing-only  sensor,  without  wasting  maneuver  ef¬ 
fort  beyond  the  minimum  necessary  for  accomplishment  of  tasks  such  as  landing  or 
weapons  employment. 

In  order  to  be  implemented  beyond  theory,  considerations  of  noise  and  flight  dis¬ 
turbances  mandate  the  need  for  the  trajectory  planning  capability  to  be  part  of  an 
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on-line  system  with  feedback,  made  fast  and  simple  enough  to  be  applied  iteratively 
in  combination  with  an  estimation  filter  using  own-ship  position  and  target  bearing 
measurement  data.  A  real-time  optimal  control  (RTOC)  system  is  designed  using  an 
Unscented  Transformation  (UT)  based  filter  for  target  estimation  coupled  with  an  ef¬ 
ficient  pseudospectral  method  (PSM)  algorithm  designed  around  the  same  principles. 

System  stability  and  precision  are  validated  through  Monte  Carlo-style  simulation. 
An  existing  quadrotor  helicopter  is  then  extensively  modified  to  allow  application  of 
the  RTOC  system,  and  effectiveness  and  feasibility  are  verified  through  flight  test, 
guiding  the  quadrotor  to  a  wire  and  landing  upon  it.  Several  integration  techniques 
are  created  and  presented  that  should  be  considered  in  an  effort  to  apply  a  RTOC 
system  to  actual  hardware. 

1.4-1  Contributions. 

Several  significant  contributions  to  the  held  of  science  have  been  made  in  the 
accomplishment  of  this  work: 

•  The  most  significant  contribution  is  a  fundamentally  new  method  of  approach¬ 
ing  the  bearing-only  trajectory  optimization  problem.  A  myriad  of  small  varia¬ 
tions  have  been  applied  to  this  problem,  all  of  them  centering  around  optimiz¬ 
ing  some  scalar  information  metric.  This  work  provides  the  ability  to  achieve 
a  predetermined  final  certainty  in  a  target  estimate,  while  simultaneously  ac¬ 
complishing  a  primary  mission  beyond  pure  localization.  Prescribing  a  final 
certainty  level  as  a  constraint  allows  any  general  cost  function  to  be  used  for 
primary  guidance  of  the  vehicle,  as  most  appropriate  for  a  given  system  and 
its  primary  mission,  while  guaranteeing  that  the  physical  certainty  required 
for  the  navigation  needs  of  that  mission  will  also  be  met.  The  method  does 
not  suffer  from  the  problematic  loss  of  directional  information  caused  by  scalar 
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compression  of  an  information  metric  in  other  methods.  The  key  enabler  for 
this  technology  is  the  ability  to  estimate  the  effects  of  discrete  measurement 
updates  with  information  states  in  a  polynomial  space,  allowing  propagation 
of  geometric  certainty  information  in  relation  to  time  within  the  context  of  the 
optimal  control  problem. 

•  This  work  also  contributes  a  physically  realizable  RTOC  algorithm  for  a  system 
with  moderate  dynamics  using  pseudospectral  methods  that  are  tailored  to  have 
a  coherent  effort  with  an  estimation  filter.  The  same  underlying  principles  of  the 
Unscented  Kalman  Filter  (UKF)  designed  for  this  work  are  incorporated  into 
the  optimal  control  problem.  Specific  computational  issues,  such  as  singularity 
avoidance,  are  addressed  in  order  to  allow  a  method  for  a  PSM  to  be  used  to 
control  a  vehicle  to  an  unknown  final  boundary  condition.  Beyond  simulation, 
this  work  fleshes  out  all  of  the  details  from  concepts  and  theory  to  hardware 
implementation. 

•  The  application  of  pseudospectral  methods  is  new  to  the  held  of  real-time  op¬ 
timal  control,  and  has  seen  little,  if  any,  application  beyond  simulation  for  sys¬ 
tems  with  moderate  dynamics.  Current  trends  in  the  RTOC  community  include 
speeding  up  an  outer  trajectory  planning  loop  to  the  point  where  control  can 
be  applied  in  a  recursive,  open-loop  manner,  re-planning  the  optimal  path  fast 
enough  to  achieve  the  equivalent  of  optimal  feedback  control.  While  effective  in 
the  simulation  environment,  research  for  this  project  highlighted  significant  lim¬ 
itations  in  these  techniques.  Removal  of  the  classical  feedback  concepts,  while 
tempting,  loses  the  insights  gained  from  integration  of  path  error,  making  the 
system  unable  to  properly  respond  to  non-zero  mean  disturbances.  A  return 
to  the  classical  application  of  optimal  control  with  an  error  feedback  loop  is 
proposed,  with  the  addition  of  an  error  bias  feedback  loop  to  the  path  planner 
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enabling  the  system  to  respond  to  a  stochastic  environment  in  an  optimal  man¬ 
ner.  This  method  should  be  adopted  as  the  industry  standard  for  application 
of  future  RTOC  algorithms,  independent  of  the  numeric  solution  method  used. 


•  A  significant  advantage  in  RTOC  is  provided  through  allowing  an  unknown 
calculation  time  for  the  optimization  cycle,  making  use  of  new  solutions  as  soon 
as  they  are  available.  The  necessary  implementation  tools  of  tip/tail  blending 
and  variable-rate  loop  integration  are  developed.  Though  more  complex,  the 
optimal  path  update  rate  is  increased  markedly,  greatly  improving  the  flexibility 
and  response  to  uncertainty  for  any  RTOC  system. 

•  Finally,  the  algorithm  produced  also  provides  the  community  with  a  planning 
tool  likely  to  be  needed  as  power  line  landings  become  more  of  a  possibility. 
Recognizing  that  some  very  small  systems  will  not  have  the  computational 
capacity  and  energy  for  RTOC,  this  tool  provides  a  way  to  find  and  extract 
the  key  characteristics  of  the  optimal  path.  The  general  shape  and  decision 
points  of  the  solution  will  vary  from  system  to  system  by  dynamics,  scale,  and 
speed  limitations.  By  using  the  simulation  provided  herein,  the  trajectories  may 
be  run  for  the  specifics  of  a  particular  system,  and  heuristics  can  be  built  which 
mimic  the  optimal  solution  without  the  computational  burden. 

Application  of  this  technology  to  modern  systems  translates  into  a  first  shot  op¬ 
portunity  for  a  submarine,  a  higher  end-game  energy  for  a  HARM,  or  the  ability  to 
land  a  sUAS  on  a  power  line  for  energy  harvesting. 
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1.5  Document  Outline 


All  of  the  major  concepts  involved  in  this  research  are  presented  in  this  document, 
to  a  level  of  detail  that  should  allow  reconstruction,  if  desired.  Chapter  |TT|  presents 
the  relevant  current  state  of  the  art  in  the  field  of  optimal  control.  Limitations  of 
computation  time  and  inadequacy  for  stochastic  problems  are  discussed,  and  direct 
methods  of  transcription  and  collocation  are  detailed  as  potential  techniques  to  speed 
up  the  process  enough  for  real-time  implementation.  Past  efforts  in  the  area  of  trajec¬ 
tory  optimization  are  covered,  as  well  as  attempts  to  combine  trajectory  optimization 
with  optimal  control  in  dual  control  methods.  Real-time  efforts  and  limitations  are 
discussed  throughout  the  chapter. 

Chapter  [TTT] describes  the  details  of  the  specific  land-on-a-wire  problem,  and  scopes 
the  region  of  interest.  The  most  relevant  coordinate  systems  used  and  the  dynamics 
and  measurement  models  are  introduced  in  the  Cartesian  system,  and  transformed 
into  the  polar  coordinate  system  for  use  with  the  hybrid  Extended  Kalman  Fil¬ 


ter  (HEFK)  and  the  shooting  method  later  developed.  Chapter  IV  addresses  the 
bearing-only  estimation  problem,  and  develops  the  HEKF  and  an  Unscented  Kalman 
Filter  as  estimation  options.  The  HEKF  was  used  for  a  large  portion  of  the  research 
and  is  available  for  future  users  who  desire  the  final  covariance  limitations  in  the  polar 
format  without  additional  non-linear  transformations.  The  filter  that  was  selected  for 
the  final  flight  tests  was  based  on  the  Unscented  Transformation. 

In  Chapter  [Vj  the  question  is  addressed  of  how  to  get  the  information  from  the 
discrete  measurement  updates,  and  the  geometry  from  which  they  were  taken  from, 
encapsulated  into  a  form  which  an  optimal  solver  could  use  to  determine  how  to 
adjust  an  optimal  path.  Information  states  are  developed  that  are  polynomial  ap¬ 
proximations  of  elements  in  the  Fisher  Information  Matrix.  These  are  used  to  allow 
the  optimal  solver  to  have  an  information  gradient  for  how  to  change  the  path,  and  to 
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allow  application  of  the  final  required  covariance  as  a  boundary  condition.  From  this, 
the  optimal  control  problem  is  constructed  with  the  information  states  augmenting 
the  system  model. 

With  a  single-shot  solution  in  hand,  Chapter  |VI| focuses  on  the  structure  of  RTOC 
implementation,  specifically  addressing  a  current  RTOC  practice  of  equating  recursive 
open-loop  solutions  with  feedback  control  when  the  solutions  are  available  fast  enough. 
Case  studies  are  provided  as  counter  examples,  and  a  structure  that  is  more  effective 
in  the  face  of  real-world  biases  and  time-correlated  disturbances  is  presented. 


Chapter  VII  implements  that  structure  in  the  full  RTOC  algorithm  used  for  the 
land-on- a-wire  problem,  addressing  hardware  considerations  such  as  discontinuities 
and  timing.  Use  of  a  variable  calculation  time  is  shown  to  increase  system  flexibil¬ 
ity  and  responsiveness  by  increasing  the  optimal  solution  update  rate,  and  poten¬ 
tial  issues  with  doing  this  are  managed.  Radau  Pseudospetral  Methods  are  used  to 
transcribe  the  continuous  optimal  control  problem  into  a  non-linear  programming 
problem,  and  adaptive  grid  refinement  is  used  to  further  increase  the  solution  speed. 


Chapter  |VIII|  describes  the  actual  quadrotor  helicopter  system,  as  well  as  the 
flight  control  modifications  required  to  enable  the  actual  flight  test.  The  results  of 


the  flight  tests  are  presented  with  analysis  in  Chapter  IX,  along  with  results  from  a 
Monte  Carlo-style  simulation  that  looks  at  robustness  and  accuracy.  The  conclusions 
drawn  from  the  results,  as  well  as  recommendations  for  future  work,  are  found  in 
Chapter  |Xj  For  reconstruction,  the  flight  control  simulator  Simulink  model  is  pre¬ 
sented  in  Appendix  [Aj  and  selected  portions  of  the  Matlab®  code  for  the  RTOC 
algorithm  that  may  be  of  particular  interest  are  presented  in  Appendix  |B} 
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II.  Related  Work 


/  I  he  primary  focus  of  this  dissertation  is  the  design  of  a  real-time  optimal  con- 
trol  algorithm  capable  of  commanding  and  updating  an  optimal  path  for  a 
sUAS  to  perch  on  a  wire  with  bearing-only  measurement  data,  considering  current  and 
required  uncertainty  levels  in  the  definition  of  optimality.  Making  the  system  imple- 
mentable  requires  the  integration,  application,  and  expansion  of  existing  knowledge 
from  several  broad  and  often  overlapping  areas,  including  optimization,  non-linear 
flight  dynamics,  aircraft  control,  navigation,  and  estimation.  Many  areas  where  work 
was  required  that  is  not  expected  to  be  contributory  to  the  body  of  knowledge  are 
not  highlighted  in  this  chapter. 


2.1  Optimal  Control 

Optimal  control  and  trajectory  optimization  have  been  studied  for  centuries.  In 
essence,  it  is  the  search  for  the  set  of  control  signals  that  will  minimize  (or  maximize) 
some  performance  criterion  while  satisfying  some  physical  constraints  [66].  The  roots 
of  optimal  control  rest  in  the  Calculus  of  Variations,  formulated  by  giants  such  as 
Bernoulli,  Newton,  Leibniz,  Euler,  and  Lagrange  [10].  Great  strides  occurred  in  the 
1800’s,  when  Hamilton  and  Jacobi  formalized  the  concept  of  a  differential  equation 
governing  the  partial  derivative  of  an  objective  function  with  respect  to  the  parameters 
of  a  family  of  extremals  (we’d  call  them  states).  Legendre,  Clebsh,  and  Weierstrass 
followed  by  refining  the  necessary  conditions  for  a  true  optimal  solution,  and  by  the 
early  1900s,  Bolza  and  Bliss  had  built  the  structure  of  the  Calculus  of  Variations  to 
its  present  form  m 
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As  is  usually  the  case,  technological  advancements  opened  up  new  needs  for  engi¬ 
neering  solutions,  and  the  space  race  of  the  1950s  brought  the  next  jump  in  optimal 
control  with  the  work  of  Soviet  Lev  Semanovich  Pontryagin  [89],  with  his  maximum 
principle,  and  American  Richard  Bellman  [3],  known  best  for  his  work  in  dynamic 
programming.  As  control  systems  became  more  digital  and  computers  more  prolific, 
Rudolf  Kalman  hugely  expanded  the  practical  applicability  of  optimal  theory  when  he 
found  an  optimal  state  feedback  gain  through  solving  the  backward  Riccati  equation 
on  an  infinite  time  horizon  for  Multiple-Input,  Multiple-Ouput  (MIMO)  systems  [60j. 

2.1.1  Limitations  of  Optimal  Control. 

Kalman’s  feedback  gain  process,  later  dubbed  the  Linear  Quadratic  Regulator 
(LQR),  and  its  sister,  the  Linear  Quadratic  Estimator  (LQE),  were  especially  sig¬ 
nificant  [2J.  For  linear  systems  (or  reasonably  linearizable  systems),  a  solution  for 
optimal  feedback  was  now  practical  and  realizable,  with  appropriate  attention  to 
robustness  [13]. 

Unfortunately,  an  optimal  feedback  solution  is  often  not  available  for  systems  with 
complexities  such  as  non-linear  problem  spaces,  intricate  cost  functions,  time-varying 
physical  constraints,  or  problems  where  knowledge  of  the  objective  is  dependent  on 
the  path  chosen  (such  as  simultaneous  trajectory  optimization  and  localization).  In 
this  case,  the  basic  practice  is  to  numerically  solve  the  optimal  control  problem  in 
an  open-loop  sense  a  priori ,  assuming  the  boundary  conditions  that  will  exist  when 
the  control  is  applied.  The  optimal  control  is  then  applied  and  disturbances  are 
rejected  by  feeding  back  the  error  between  the  expected  optimal  path  and  the  current 
position  [62] .  Stability  and  feasibility  are  maintained  if  the  system  remains  “close 
enough”  to  the  nominal  path. 
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For  cases  where  the  exact  state  at  the  time  the  control  will  be  needed  is  not  fore¬ 


known,  and  for  cases  where  disturbances,  model  inaccuracies,  or  noisy  measurements 
cause  large  deviations  from  the  optimal  path,  an  ability  to  recursively  solve  the  prob¬ 
lem  with  new  information  is  intuitively  desirable.  The  drawback,  historically,  has 
been  the  extensive  calculation  time  required  to  numerically  solve  an  optimal  control 
problem.  Methods  dubbed  shooting,  multiple  shooting,  genetic  algorithms,  simulated 
annealing,  particle  swarm  optimization,  and  others  have  been  used  with  varying  speed 
and  numerical  stability.  The  most  promising  techniques  have  included  parameteriza¬ 
tion  of  the  problem  into  a  finite  solution  space,  as  is  accomplished  in  direct  methods. 


2.2  Direct  Methods 

Optimal  control  methods  can  be  generally  categorized  into  either  direct  or  indi¬ 
rect  [5].  Indirect  methods  involve  determining  extremals  with  the  Hamiltonian  and 
first-order  optimality  conditions  [T3l  [66j .  While  these  methods  offer  great  insight  into 
the  problem,  they  have  several  drawbacks.  First,  indirect  methods  cumulatively  eval¬ 
uate  the  objective  function  (and  its  gradient)  over  the  entire  trajectory,  as  opposed 
to  direct  methods  which  do  this  only  at  several  points.  Direct  methods,  therefore, 
have  more  information  on  where  to  apply  changes  to  the  initial  guess,  resulting  in 
larger  radii  of  convergence  than  with  indirect  methods,  which  require  a  good  (and 
generally  nonintuitive)  initial  guess  of  both  states  and  costates  [7].  In  addition,  if  the 
problem  is  constrained,  the  indirect  method  requires  breaking  the  problem  into  con¬ 
strained  and  unconstrained  arcs,  which  may  not  be  known  a  priori  [5j.  In  addition, 
indirect  methods  are  often  extremely  sensitive  to  problems  with  unknown  boundary 
conditions  [TT| . 
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Direct  methods  are  more  robust  to  errors  in  the  initial  guess,  more  computationally 
efficient,  and  apply  to  a  larger  range  of  problems.  Euler  was  the  Erst  to  create  what 
we  now  call  the  direct  method  of  finite  differences,  though  the  method  lay  dormant 
for  quite  some  time  [26].  Direct  methods  transcribe  the  optimal  control  problem 
into  a  non-linear  programming  problem  (NLP),  which  in  modern  days  is  then  solved 
numerically  mm- 

2.2.1  Transcription  and  Collocation. 

The  underlying  technique  for  a  direct  method  is  collocation,  or  transcription — 
terms  which  are  often  used  interchangeably.  The  state  vector  is  approximated  and 
represented  by  a  discrete  number  of  variables  (e.g.,  coefficients  of  a  Fourier  series). 
The  continuous  dynamic  constraints  for  the  system  are  then  evaluated  at  select  col¬ 
location  points,  or  nodes,  producing  a  discrete  number  (albeit  typically  a  large  num¬ 
ber)  of  static  equations — one  for  every  state,  at  every  node  [86] .  These  constraints 
are  used  to  form  a  new,  static  optimization  problem,  seeking  a  vector  of  state  and 
control  variables  at  each  collocation  point  to  minimize  the  overall  cost  while  obeying 
each  of  the  new  static  constraints.  In  essence,  the  problem  has  been  transformed 
from  an  infinite- dimensional  optimization  problem  to  a  finite- dimensional,  non-linear 
programming  problem  p3] .  There  is  no  guarantee  that  the  optimality  or  the  dynamics 
hold  at  other  than  the  collocation  points  non,  but  I.  M.  Ross  has  shown,  for  an 
increasing  number  of  nodes,  that  “If  the  optimal  solution  of  the  discrete  problem 
converges,  it  must  converge  to  an  optimal  solution  of  the  continuous  problem  HQ.” 
After  conversion  to  an  NLP,  the  problem  can  be  solved  with  a  host  of  solvers  designed 
for  this  purpose  such  as  SNOPT  [39],  SPRNLP  [6],  or  KNITRO  [15],  most  of  which 
use  Sequential  Quadratic  Programming  (SQP)  as  the  primary  solution  method  and 
account  for  matrix  sparseness  with  a  semi-definite  reduced-Hessian. 
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2.2.2  Pseudospectral  Methods. 


Instead  of  directly  discretizing  a  state  or  control  history,  the  number  of  optimiza¬ 
tion  parameters  can  be  decreased  by  parameterizing  the  vector  using  a  series: 

N 

u0)  =  ^2ci<f>i{t)  (i) 

i=l 

The  constants,  <y,  are  the  parameters  solved  appropriate  for  the  set  of  basis  functions, 
{(pi{t)}f=1.  If  orthogonal  polynomials  are  used  as  the  basis  functions,  and  the  zeros  of 
orthogonal  polynomials  (or  their  derivatives)  are  used  for  the  collocation  points,  the 
method  is  dubbed  pseudospectral  [25)[96j.  Using  polynomials  allows  trivial  differenti¬ 
ation,  which  makes  enforcement  of  the  dynamic  constraints  more  efficient  than  other 
direct  methods  which  rely  on  integration  to  approximate  the  vector  field  [56 J . 

Pseudospectral  methods  had  their  origin  in  spectral  methods,  a  technique  for  solv¬ 
ing  partial  differential  equations  referenced  as  far  back  as  Reddien  in  1979  (ST]  and 
used  extensively  in  the  realm  of  fluid  dynamics  [16].  The  ideas  migrated  into  control 
theory  in  the  field  of  chemical  engineering  with  the  work  of  Cuthrcll  (among  oth¬ 
ers)  [20] .  Within  recent  years,  the  application  of  pseudospectral  methods  to  optimal 
control  has  grown  quickly,  and  the  frequency  of  journal  articles  on  the  subject  has 
had  a  sharp  rise.  At  least  in  simulation,  pseudospectral  methods  have  been  applied 
to  the  control  of  platforms  spanning  from  cars  mi  to  hypersonic  reentry  vehicles  [58j . 

There  has  been  a  great  deal  of  development  and  refinement  of  PSM,  resulting  in 
three  primary  varieties,  the  Legendre-Gauss-Lobatto  Pseudospectral  Method  (LPM), 
the  Gauss  Pseudospectral  Method  (GPM),  and  the  Radau  Pseudospectral  Method 
(RPM).  The  fundamental  difference  stems  from  the  selection  of  collocation  points. 
Commonly,  for  problems  with  a  finite  final  time  (may  be  unknown),  the  affine  trans- 
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formation: 


tf  +  to 


t  = 


tf  —  to 


-r  + 


(2) 


is  applied  to  transform  the  problem  from  time  interval  t  G  [to,tf]  to  the  interval 
r  G  [—1,1].  The  infinite  horizon  problem  is  mapped  from  t  G  [to,  oo)  to  the  finite 
horizon  r  G  [—1,1],  but  states  and  controls  at  the  final  point  are  intentionally  not 
calculated  to  avoid  a  singularity  [271  [Ml] .  Transforming  the  time  allows  selection  of 
interpolation  points  from  the  interval  -1  to  1.  The  distinction  is  made  between  state 
interpolation  points,  which  include  the  endpoints  r  =  —1  and  r  —  1,  and  the  colloca¬ 
tion  points,  where  the  dynamic  constraints  are  applied  [35] .  GPM  does  not  collocate 
at  either  endpoint,  but  only  at  the  interior  Legendre-Gauss  (LG)  points.  This  style 
of  collocation  leads  to  a  set  of  discrete  Karush-Kuhn  Tucker  (KKT)  optimality  con¬ 
ditions  identical  to  the  discretized  form  of  the  first-order  optimality  conditions  of  the 
continuous  problem  at  the  LG  points,  allowing  the  costates  to  be  accurately  esti¬ 
mated  using  KKT  multipliers  from  the  NLP  [5].  RPM  uses  Legendre-Gauss  Radau 
(LGR)  points,  which  include  one  endpoint  or  the  other  (the  non-symmetric  points 
can  be  mirrored  about  zero).  Though  the  KKT  conditions  differ,  the  method  includes 
collocation  at  an  endpoint,  reducing  the  requirement  to  solve  for  that  point  and  po¬ 
tentially  increasing  the  accuracy  of  the  solution.  Notably,  differentiation  matrices 
from  both  GPM  and  RPM  are  both  non-square  and  full  rank,  allowing  the  expression 
as  an  integration  matrix,  making  the  problem  reversible.  Costate  estimates  for  both 
GPM  and  RPM  converge  exponentially.  LPM,  which  uses  Legendre-Gauss-Lobatto 
(LGL)  points  for  collocation  (including  both  endpoints),  has  a  square,  singular  differ¬ 
entiation  matrix.  This  directly  provides  the  state  and  control  at  both  endpoints,  as 
well  as  ensuring  the  dynamics  are  met,  but  at  the  cost  of  a  potentially  non-convergent 
costate  [35] .  The  weights,  differentiation  matrices,  and  techniques  for  generation  of 
enough  constraints  differ  for  each  of  the  methods. 
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For  each  of  the  techniques,  the  orthogonal  nodes  are  not  equally  spaced,  but 
clustered  near  the  endpoints,  similar  to  Chebyshev  points.  This  spacing  minimizes  the 
Runge  phenomenon,  a  potentially  divergent  oscillation  that  can  occur  when  increasing 

the  order  of  an  interpolating  polynomial,  as  in  Figure  [6]  (65]. 

Example  of  Runge  Phenomenon 


Figure  6.  Runge  Phenomenon  as  the  Number  of  Equally  Spaced  Nodes  is  Increased 


In  addition  to  accurate  interpolation,  the  proper  selection  of  collocation  points 
also  aids  in  the  evaluation  of  the  objective  function.  With  the  states  and  control 
only  being  evaluated  at  discrete  points,  the  objective  function  can  be  quickly  cal¬ 
culated  with  quadrature,  exact  to  polynomials  of  degree  2 n  +  1,  and  guaranteed  to 
converge  for  higher  order  polynomials  to  any  continuous  function  by  the  Weierstrass 
Approximation  Theorem  [65]: 

/I  n 

f(x,  u)dx*Yl  (3) 

1  i= 1 

where  weights,  Wi,  are  selected  appropriate  to  the  collocation  scheme  (e.g.,  Gauss 
points,  Gauss  weights). 
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Controls  or  states  with  discontinuities  are  problematic,  often  suffering  from  Gibb’s 
phenomenon,  (a  large  oscillation  prior  to  a  jump  in  the  solution)  [32] .  If  the  problem  is 
known  to  be  non-smooth  (a  change  in  mass  when  a  rocket  drops  a  stage,  for  example), 
it  is  best  dealt  with  by  segmenting  at  problem  areas  with  “knots”  [96] ,  or  phases  [91] . 
These  can  also  be  used  to  mark  a  point  in  the  problem  where  the  dynamics  change. 
Since  the  nodes  are  concentrated  at  the  start  and  end  of  each  phase,  the  break  point 
will  generate  the  greatest  nodal  density,  and  the  number  of  nodes  for  each  phase  can 
be  increased  until  the  solution  is  sufficiently  accurate. 

Tsuchiya  sought  to  increase  the  density  of  nodes  in  the  first  portion  of  a  solution 
in  a  near-real-time  implementation  for  aircraft  guidance.  Recursive  solutions  were 
provided  every  30  seconds.  Assuming  convergence  of  the  next  path,  only  the  first  30 
seconds  of  each  provided  path  was  flown.  An  introductory  segment  of  fixed  time  was 
declared,  with  a  higher  node  density  to  provide  smoother  control  for  the  portion  of 
the  path  that  would  actually  be  used  m- 

2.2.2. 1  Adaptive  Grid  Refinement. 

Darby  has  contributed  an  hp-Adaptive  method  that  adjusts  griding  on  the  fly, 
even  for  systems  where  the  shape  of  the  solution  is  not  known  [2T] .  Finite  element 
“hp”  methods  were  adapted,  where  h  refers  to  the  segment  width  and  p  denotes  the 
order  of  the  polynomial  degree  in  each  segment.  Recalling  that  the  dynamics  of  the 
states  and  controls  are  only  enforced  at  the  collocation  points,  Darby  calculates  the 
same  collocation  constraint  (the  derivative  of  the  approximating  polynomial  must 
match  the  derivative  supplied  from  the  dynamics),  but  the  constraint  is  evaluated 
between  collocation  points,  forming  a  matrix  of  midpoint  residuals.  Oversimplifying, 
if  a  single  residual  is  high,  a  discontinuity  is  suspected  and  a  segment  break  is  added 
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for  the  next  iteration.  If  many  residuals  are  high,  a  poor  polynomial  fit  is  assumed 
and  p  is  increased. 

This  method  was  adopted  for  the  real-time  controller  in  this  project.  Accomplish¬ 
ing  collocation  in  this  manner  allows  fewer  nodes  to  be  used  in  attaining  the  initial 
solution,  without  fear  of  missing  important  characteristics  in  the  optimal  path,  as 
differences  between  nodes  will  be  checked.  Fewer  nodes  translates  to  a  less  complex 
NLP,  solved  with  a  greater  speed.  While  more  solution  iterations  are  required,  each 
iteration  “bootstraps”  the  guess  from  its  predecessor,  greatly  aiding  convergence. 

2.2.3  Real-Time  Implementation  Methods. 

Recent  efforts  to  apply  optimal  control  in  real-time  are  increasing.  In  cases  where 
a  feedback  law  (LQR,  LQG,  etc.)  cannot  be  formed,  a  partial  solution  can  be  used  for 
some  simple  problems.  Kalmar-Nagy  found  that  for  a  simple  minimum-time  TPBVP, 
knowing  the  structure  of  the  solution  (bang-bang  in  this  case  [75])  can  sometimes  offer 
relationships  that  must  be  held  constant,  producing  a  “near  optimal”  problem  with 
greatly  reduced  order  that  can  be  solved  quickly — either  completely  open-loop,  or 
partially  closed  [6Tj. 

Benson  recognized  another  potential  technique  using  the  Gaussian  pseudospectral 
method  for  real-time  control  [4] .  His  novel  idea  hinged  on  the  recognition  of  the  avail¬ 
ability  of  an  accurate  costate  from  the  method,  particularly  the  initial  costate,  even 
with  a  small  number  of  nodes.  Assuming  the  state,  x,  dynamics,  f ,  time,  t,  costate,  A, 
Hamiltonian,  R,  control  u,  and  the  set  of  admissible  controls,  U,  the  relationships  for 
the  state  and  costate  are  found  through  the  familiar  first-order  necessary  conditions: 


(4) 


(5) 
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Further,  Pontryagin’s  maximum  principle  supplies  the  optimal  control: 

u (t)  =  arg  min  \H  (x(£),  A (f),  u (t),t)]  (6) 

ueu 

Benson’s  unique  concept  is  to  use  the  pseudospectral  method  to  determine  the 
initial  value  for  the  costate.  This  value  is  combined  with  a  measurement  of  the  actual 
state  to  determine  the  current  control  with  Equation  [6]  The  value  for  control,  along 
with  the  state  measurement  and  the  pseudospectral  approximation  for  the  initial 
costate,  are  used  to  propagate  the  time  derivative  of  the  costate  with  Equation  [5j 
using  a  single  step  numerical  technique  such  as  a  Runge-Kutta  integration  scheme  [65] . 
As  the  costate  is  propagated,  the  control  is  continually  updated  with  Equation  [6]  As 
disturbances  and  modeling  errors  alter  the  current  state  from  the  optimal  trajectory, 
the  optimal  problem  is  re-solved  using  the  current  state  as  the  initial  condition  to  find 
a  new  estimate  for  the  current  (now  the  new  initial)  costate.  The  costate  propagation 
is  re-initialized  with  this  value  and  the  recursion  continues.  Of  course,  this  solution 
assumes  that  the  cost  function  is  not  time-varying. 

Gong  and  1.  M.  Ross  outline  a  different  style  of  recursive  feedback — certainly  the 
most  popular,  and  arguably  the  simplest.  A  real-time  optimal  trajectory  planner 
produces  an  outer-loop  reference  trajectory  as  quickly  as  possible,  while  a  linear  or 
non-linear  controller  maintains  the  reference  trajectory  until  the  next  update.  The 
concept  is  that  if  the  outer-loop  reference  trajectory  can  be  calculated  quickly  enough, 
the  inner  loop  can  be  removed  m  [95].  The  comparison  is  made  between  simple 
sample  and  hold  style  discrete  control  and  the  forward-looking,  open-loop  solutions, 
repeatedly  applied,  referred  to  as  “Caratheodory-7r  feedback”  [95] .  The  conclusion  is 
reached  that  with  fast  enough  open-loop  solutions,  the  search  for  a  closed-loop  feed¬ 
back  can  be  abandoned  pH]  (this  conclusion  will  be  challenged  in  Chapter  |VI[).  When 
the  outer  loop  is  not  “fast  enough,”  errors  will  occur  in  the  initial  conditions.  An 
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assumed  initial  condition  is  seeded  to  the  trajectory  planner  (based  on  expectations 
from  the  last  optimal  plan),  but  disturbances,  unknowns,  and  differing  computation 
times  will  change  the  actual  value  of  the  state  when  the  new  optimal  solution  becomes 
available.  Yan  and  Strizzi  |!119i  [106]  have  implemented  Bryson’s  neighboring  optimal 
control  law  mm  using  an  indirect  method  in  an  effort  to  correct  for  small  devi¬ 
ations  from  the  assumed  conditions.  This  technique  was  replaced  with  cosine  wave 
smoothing  for  this  project. 

The  intent  of  this  research  is  to  build  upon  these  efforts  in  the  area  of  real¬ 
time  optimal  control.  The  developed  methods  will  be  applied  to  solving  a  classical 
estimation  problem — trajectory  optimization  for  bearing-only  target  analysis. 


2.3  Trajectory  Optimization 


Trajectory  modification  for  the  purpose  of  localization  and  bearing-only  tracking 
(BOT)  has  been  implemented  in  the  submarine  community,  at  least  at  the  heuristic 
level,  for  at  least  60  years  m-  The  ability  to  estimate  range  with  only  an  angle 
sensor  is  intuitively  dependent  on  the  geometry  from  which  the  measurements  are 


taken,  as  was  shown  in  Figure  2  on  page  4  Most  efforts  to  increase  the  efficacy 
of  the  observer’s  trajectory  on  target  state  estimation  have  attempted  to  optimize  a 
path  based  on  control  from  two  general  categories — pure  localization  theory,  and  dual 
control  theory,  typically  a  suboptimal  hybrid  of  estimation  and  optimal  control.  Both 
methods  rely  on  the  principles  of  pure  localization,  and  trajectories  are  optimized 
based  on  some  representation  of  target  position  information,  such  as  the  Fisher  Infor¬ 
mation  Matrix  (FIM),  the  Cramer- Rao  Lower  Bound  (CRLB),  or  an  estimated  error 
covariance.  One  limitation  of  these  techniques  is  the  loss  of  the  full  directional  quality 
that  should  be  guiding  the  trajectory  when  the  information  reference  is  compressed 
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into  a  scalar  performance  index.  There  has  been  a  great  amount  of  effort  invested  in 
attempting  to  find  out  which  cost  functional  is  least  affected  by  this  limitation. 

2.3.1  Localization  and  Bearing-only  Tracking. 

Lindgren  [72] ,  with  Nardone  and  Aidala  [82]  laid  some  of  BOT  problem’s  founda¬ 
tional  groundwork  in  the  submarine  context  by  developing  criteria  for  observability. 
Efforts  to  increase  observability  began  with  “two  leg”  options,  looking  for  a  “lead-lag” 
trajectory  ra,  or  fixing  the  heading  for  the  initial  leg  and  optimizing  the  heading  for 
the  second  leg  H- 

Hammel  expanded  on  this  [J7.  175] .  pushed  the  BOT  processing  algorithms  Iji:. 
and  investigated  the  application  to  trajectory  planning  by  maximizing  an  analytic 
approximation  of  the  determinant  of  the  FIM.  The  FIM  provides  a  measure  of  the 
amount  of  information  that  is  obtained  from  measurements,  and  is  a  function  of 
the  geometry  of  the  problem,  rather  than  the  estimation  method.  Maximizing  the 
determinant  of  the  FIM  effectively  minimizes  the  volume  of  the  uncertainty  ellipsoid 
around  the  target  position  estimate. 

Hammel’s  method  for  optimal  control  problem  formulation  became  the  standard 
approach — the  continuous  problem  was  parameterized,  assuming  the  observer  to  have 
a  constant  speed  and  infinite  heading-rate  ability.  A  preset  number  of  equal  length, 
constant  heading  segments  was  then  assumed,  reducing  the  optimal  control  to  a  single 
sequence  of  headings  to  apply  to  the  segments.  Note  that  a  constant  velocity  and  fixed 
final  time  (indirectly  assumed  through  a  fixed  number  of  equal  duration  segments) 
are  common  assumptions  made  in  these  techniques  for  tractability.  This  represents 
a  major  shortcoming — in  effect,  when  solving  for  the  optimal  path,  the  sensitive 
parameters  of  path  length  and  the  number  of  measurements  must  be  provided  as 
assumed  inputs,  though  they  greatly  change  the  nature  of  the  solution.  Figure  [7] 


demonstrates  this  with  plots  from  Hammel  [20J  and  Oshman  [STj,  where  both  VT/r0 
and  K  represent  a  required  solution  input  parameter  of  the  ratio  of  total  path  length 
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Fig.  3:  Optimal  trajectories  for  varying  K 


(b)  Families  of  Solutions  Varying  K  [S3] 


Fig.  4:  Observer  trajectories 

Figure  7.  Effect  of  Specifying  Path  Eelngth  <yn  Localization 
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rameters  by  approximating  the  control  vector  with  orthogonal  bagis  functions),  and 
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with  a  differential  inclusion  method  (removing  control  by  replacing  it  with  a  state 
constraint,  such  as  an  equation  for  constant  velocity)  [53].  Liu  used  a  suboptimal 
approach,  analytically  maximizing  a  lower  bound  on  the  determinant  of  the  FIM, 
vice  the  determinant  itself  m 

Faced  with  problems  that  stem  from  compression  of  the  information  metric  into 
a  scalar,  Helferty  moved  away  from  the  determinant  of  the  FIM  j50j.  Minimizing 
the  scalar  uncertainty  volume  (or  the  area,  for  this  particular  2-D  case)  was  found 
to  produce  solutions  that  may  favor  highly  eccentric  confidence  ellipsoids.  This  is 
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especially  problematic  for  localization  problems,  where  the  largest  ambiguity  axis 
often  corresponds  to  the  unknown  range  variable,  where  most  of  our  attention  is 
needed.  Minimizing  the  trace  of  the  CRLB  was  suggested  instead.  The  CRLB  is  a 
lower  bound  on  the  error  covariance  of  the  estimation  problem.  It  represents  the  best 
certainty  attainable  from  measurements  along  that  path ,  not  necessarily  what  could 
be  obtained  by  some  other  path,  and  by  definition  is  the  inverse  of  the  FIM  for  an 
efficient  estimator.  With  each  eigenvalue  corresponding  to  the  square  of  one  axis 
of  the  confidence  ellipsoid,  the  trace  (sum  of  the  eigenvalues)  yields  the  sum  of  the 
squares  of  each  axis.  Therefore,  minimizing  it  penalizes  solutions  with  a  large  axis  of 
uncertainty  resulting  in  less  ambiguity  of  optimal  solutions  [|49j .  Logothetis  developed 
a  similar  “mutual  information  metric,”  the  maximization  of  which  was  equivalent  to 
the  minimization  of  the  CRLB  determinant  m • 

The  trace  of  the  FIM  has  at  times  been  selected  as  the  metric  of  choice  and  efficient 
to  calculate,  but  has  also  been  shown  to  be  unstable  and  potentially  singular  [SB]. 
Le  Cadre  created  an  approximation  of  the  FIM  that  was  additively  monotonic,  and 
then  took  the  trace  of  the  approximation  m  He  later  followed  the  concept,  allowing 
for  maneuver  of  the  target  using  a  hidden  Markov  model  (HMM),  and  determined  the 
optimal  heading  sequence  with  classical  dynamic  programming  |[68j.  More  recently, 
Per  Skoglar  used  a  steepest  descent  method  for  the  optimization  and  a  particle  filter 
for  the  estimation.  For  the  Gaussian  case,  he  showed  that  trajectory  planning  with 
the  determinant  of  the  FIM  was  equivalent  to  using  the  differential  entropy  of  the 
posterior  target  density  HE].  In  the  context  of  multiple  robots  using  Model  Predictive 
Control  (MPC),  Leung  chose  to  maximize  the  minimum  eigenvalue  of  the  FIM  for 
localization  uni. 

Ponda  compared  solutions  using  several  of  the  most  popular  FIM  metrics  (deter¬ 
minant,  maximum  eigenvalue  of  the  inverse,  negative  trace,  and  trace  of  the  inverse) 
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in  the  context  of  the  same  problem — determining  the  location  of  a  ground  target 
optically  with  a  sUAS,  allowing  100  measurements  in  a  fixed  path  length  [88].  Un¬ 
surprisingly,  the  determinant  of  the  FIM  was  found  to  no  longer  contain  information 
about  the  angular  dependence  between  the  measurements  (compression).  Maximizing 
the  trace  of  the  FIM  was  better,  and  avoided  some  local  minimum  problems  along 
a  single  path,  but  found  to  be  unstable  and  have  the  potential  to  result  in  a  singu¬ 
larity.  The  largest  eigenvalue  of  the  inverse  of  the  FIM  (minimizing  the  largest  axis 
of  the  uncertainty  ellipsoid),  and  the  trace  of  the  inverse  of  the  FIM  (minimizing  the 
average  variance  of  the  estimates)  yielded  similar  results,  with  faster  convergence  and 
higher  stability  in  the  optimization.  The  final  metric  was  preferred.  In  simulation, 
Ponda  found  that  increasing  the  allowed  number  of  measurements  led  to  a  growing 
number  of  local  extrema  with  severe  sensitivities  to  initialization.  As  must  often  be 
done  in  the  world  of  optimization,  impractical  results  were  avoided  by  initializing  the 
optimization  close  to  the  global  minimum,  which,  of  course,  is  problematic  for  real 
applications. 

Note  that  a  common  thread  in  all  of  these  cases  is  that  a  scalar  approximation  of 
the  information  metric  is  the  cost  functional  that  is  optimized.  Regardless  of  which 
particular  metric  is  used,  all  of  them  suffer  from  the  loss  of  some  directional  infor¬ 
mation  when  a  scalar  is  produced  from  a  multi-dimensional  information  matrix.  The 
effort  to  minimize  this  unavoidable  effect  is  one  reason  for  the  variety  of  approaches. 
Another  common  theme  is  that  bearing-only  tracking  and  localization  techniques  se¬ 
lect  guidance  purely  for  better  estimation  of  the  target  location.  The  actual  path 
that  is  selected  is  of  no  consequence,  excepting  that  the  path  must  be  restricted 
from  reaching  the  target,  else  the  optimal  information  gathering  technique  becomes 
collision  (information  from  bearing  measurements  will  be  shown  to  be  inversely  pro¬ 
portional  to  the  square  of  range).  The  UAV  scenarios  accomplish  this  by  mandating  a 
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fixed,  planar  altitude  above  the  target  and  optimizing  over  a  receding  horizon,  and  the 
submarine  and  robot  scenarios  typically  choose  a  fixed  final  time  indirectly,  short  of 
that  required  to  reach  the  target  of  interest.  Unfortunately,  such  solutions  are  highly 
dependent  on  the  time  horizon  selected,  making  “optimality”  more  of  a  mathematical 
construct  than  a  practical  reality. 

2.3.2  Dual  Control  Theory. 

The  previous  references  are  examples  of  optimizing  a  trajectory  to  increase  the 
quality  of  estimation,  without  concern  for  the  actual  direction  of  the  path.  The 
converse  can  be  seen  in  optimal  problems  that  still  seek  to  estimate  the  target  location, 
but  without  reference  to  the  geometric  effect  of  the  path.  The  focus  may  be  simply 
on  “camera-on-target”  time  or  homing,  as  solved  with  several  methods,  such  as  direct 
collocation  [38]  [9?],  neural  networks  [37],  or  heuristics  [99l  11181  [23] . 

As  a  real-time  example,  in  [38] ,  Geiger  designed  a  controller  to  solve  for  a  string  of 
waypoints  that  would  enable  a  sUAS  to  maximize  time  above  a  target  with  a  known 
position.  The  technique  was  rooted  in  work  by  Dickmanns  1221.  with  equally  spaced 
nodes,  Hermitian  interpolation,  and  a  receding  horizon  approach.  The  solution  shape 
was  to  fly  directly  to  the  target,  then  to  perform  a  maximum  rate  turn  back  around, 
forming  a  cloverleaf  pattern  after  multiple  passes.  To  achieve  the  fixed  4  second 
update  interval,  only  7  nodes  were  used  with  a  short  20  second  “look  ahead”  time  for 
the  receding  horizon  (about  enough  time  for  one  turn).  This  represents  an  important 
step  in  real-time  optimal  control,  but  does  not  account  for  the  geometric  effects  of 
the  path  on  target  estimation  quality. 

The  work  herein  addresses  the  problem  of  accomplishing  both  efforts  simultane¬ 
ously.  Localization  is  critical,  but  so  are  the  path  characteristics — with  the  path  being 
primary.  The  submarine  example  from  the  introduction  concerns  tracking  a  contact, 


32 


but  the  primary  mission  is  often  moving  into  position  to  employ  ordnance.  Firing  a 
torpedo  is  the  mission ,  target  position  certainty  is  a  requirement.  In  the  same  way, 
the  HARM  missile  seeks  not  only  to  localize  its  target,  but  to  hit  it.  The  sUAS  must 
maneuver  to  localize  a  power  line,  but  the  real  mission  is  to  land. 

This  type  of  problem  is  by  definition  non-holonomic — achieving  the  final  state  is 
the  key,  but  that  state  is  dependent  on  the  path  taken  to  achieve  it.  The  control  and 
estimation  concepts  are  fundamentally  coupled  and  inseparable  ra-  The  system  has 
two  purposes  that  may  directly  conflict  with  each  other,  but  both  are  necessary — the 
quality  of  estimation  affects  the  quality  of  control  and  vice  versa  [fH].  This  is  ad¬ 
dressed  with  so-called  dual  control  or  dual  effects  theory  |30j .  Dynamic  programming 
and  search-based  approaches  are  the  general  solution  techniques,  but  are  commonly 
prohibitive  even  for  small  problems  pkh  66l  [13] . 

Frew  addressed  a  similar  problem  to  this  work  with  exhaustive  search.  In  guiding 
a  robot  with  an  angle  sensor,  the  problem  was  again  parameterized  to  find  a  heading 
sequence,  but  in  this  case,  a  particular  final  covariance  was  able  to  be  achieved.  This 
was  not  done  in  the  optimal  control  formulation  (a  contribution  of  this  dissertation), 
but  by  considering  the  outcomes  of  a  generated  acceptable  set  of  paths.  For  tractabil- 
ity,  only  five  turn-and-drive  segments  were  allowed  with  turns  restricted  to  one  of  five 
directions  (45°  apart  initially,  see  Figure  [8]d  for  an  example  of  three  steps  with  20° 
spacing).  The  final  covariance  in  the  target  estimate  was  then  calculated  using  a 
measurement  at  each  step  for  each  of  the  3,125  paths. 

Four  total  iterations  were  performed,  the  latter  three  centered  around  the  best 
path  of  the  previous  run,  with  the  space  between  allowable  angles  decreasing  each 
time.  The  number  of  segments  used  became  the  cost  function  (options  being  integers 
1-5,  representing  the  minimum  time  solution).  The  first  path  calculated  that  obtained 
the  required  final  covariance  was  declared  the  best  path,  because  any  additional  paths 
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Depending  on  the  trajectory-design  objective,  the  maneuver  duration  is  specified  in  one  of  two  ways.  For 
the  case  when  minimum  uncertainty  is  desired  in  fixed  time,  the  total  trajectory  duration  and  number  of 

....  4.  Core  Obsen/er-Trajectory  Design  Algorithm 

maneuvers  are  specified.  In  this  case  the  maneuver  duration  is  just  Tman  —  Ttotal/n  where  n  is  the — 
number  of  maneuvers.  For  the  fixed-accuracy  scenario,  the  maneuver  duration  is  fixed  and  the  number  of 


The  breadth-first  expansion  is  shown  in  Figure  4.5.  In  this  example  the  range  of  possible  heading  values  is 


restricted  to 
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Figure  8.  Optimization  of  a  Robot  Patji  by=  E^t^u^t^v^olSearch  |33] 


(4.24) 


where  the  superscript  i  indicates  the  set  applies  to  the  /'th  maneuver. 


on  that  round  that  also  met  the  fintelxeoTOria®ee  ree|Hi1rerlteMtre(aai,ida,ill'miyiiMfe;ly'n Would) 

they  are  generated,  point  1  is  selected  and  points  6  and  7  are  generated.  This  process  is  continued  for  all 


could  at  best  only  tie  in  terms  of 


maneuver.  The  lines  originating  from  the  endpoints  of  the  level  1  maneuvers  are  referred  to  as  level  2  and 

sionality  fast  becomes  an  issue,  aad.neweryt.nad£liticMralm;segmeaiitiiaJkiwiedmiiiaic®©asese  the 

original  node.  Because  the  target  motion  is  predicted  based  only  on  the  current  target  estimate,  the  predicted 

required  number  of  expected  covaffifKfi"  SU^f is same  for  every  node  at  a 

given  level.  In  other  words,  the  target  is  predicted  to  move  to  the  same  location  regardless  of  whether  the 


Other  authors,  attempting  to  make  the  problem  tractable,  and  sometimes  ana- 
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lytically  solvable,  have  split,  the  dimensions  in  which  control  is  optimized  for  path 
guidance  and  estimation  improvement  [57;  [1 13] .  Because  the  true  problem  is  insep¬ 
arable,  this  assumption  fundamentally  changes  the  nature  of  the  solution,  and  the 
results  can  only  be  suboptimal.  For  the  2-D  problem,  control  in  one  dimension  is 
typically  mandated,  most  often  assuming  a  constant  closure  in  the  direction  of  the 
target  for  a  known  final  time.  Motion  in  an  orthogonal  direction  is  then  solved  for  as 
a  one- dimensional  pure  localization  problem. 

In  [57],  Johnson  worked  towards  a  solution  that  could  be  used  in  real-time,  using 
simplifications  for  an  analytic  solution  to  guide  a  formation  partner  from  one  position 
to  another,  relative  to  the  flight  lead,  using  optical  information  to  better  discern  the 
given  position  of  the  flight  lead.  With  constant  speed  and  heading,  the  problem  is 
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the  same  as  static  localization.  For  control,  each  axis  was  treated  independently.  Al¬ 
titude  was  held  constant.  Relative  velocity  in  the  X-direction  was  also  constant  (an 
approximation  of  aircraft  velocity  difference  for  small  heading  crossing  angles).  The 
initial  distance  was  assumed  known,  and  direct  force  was  used  for  control.  Measure¬ 
ment  value  was  equated  with  distance  from  a  centerline.  With  these  assumptions, 
all  that  remained  was  a  one-dimensional  TPBVP  with  no  constraints,  a  known  final 
time,  and  a  linear  system  with  two  states — lateral  position  and  lateral  velocity. 

An  LQR  technique  was  used  to  solve  the  problem  analytically,  with  one  cost  term 
to  penalize  distance  from  the  Y  =  0  centerline,  and  another  term  to  encourage  it 
for  observability.  Figure  [9]  shows  the  result,  with  an  aircraft  being  directed  from  an 
initial  position  of  X  =  0). 


Figure  9.  Analytic  Dual-Control  S dlti f i oil ' cf ii  e  vt: d  by  Isolating  Each  Dimension  [5?j 


In  this  simulation,  the  follower  aircraft  changes  its  relative  position  from  [Ax  =  100  ft.  Ay  =  10  ft,  Az  =  0  ft]  to 
[Ax  =  50  ft,  Ay  =  0  ft,  Az  =  0  ft].  In  one  case  (Fig.  11),  the  relative  position  command  is  given  as  a  step  command 
at  a  time  of  20  seconds.  In  the  other  case  (Fig.  12),  the  optimal  path  given  by  Equation  (23)  is  utilized  as  the  command 

Bishopi'cliadsiaDivai(ia1ti0iffii'©fn4hii8  (djrnfflfts»n>t®(hpajfiatinga©®m«©pt'psh©5tona  imdFigure  [TO] 
A  constant  decrease  in  range  was  assumed  for  each  time  step,  but  it  was  not  tied  to 
a  direction.  Localization 
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Figure  10.  Optimal  Pursuer  Trajectory  with  Constant  Range  Decrease  for  Each  Step  [9] 
G  [5, 10],  At  each  step  k  the  pursuer  considers  the  bearing  history  up  to  and 


Much  like  Johnson  but  without  treating  control  and  estimation  efforts  com¬ 


pletely  independently,  ^gp^gg^L|^g^:onstant  velocity  toward  the  target,  and  then 
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concluded  that  the  one  or  two-steps-ahead  solutions  would  be  helpful  for  an  on-line 
system,  but  not  necessarily  close  to  the  optimal  solution.  Using  more  than  one  or 
two  steps  ahead  was  not  practically  implementable  [115j. 

Hodgson  used  differential  inclusion  to  solve  for  a  missile  path  that,  for  a  fixed  dwell 
time,  would  balance  a  cross  range  resolution  term  from  an  imaging  radar  with  the 
determinant  of  the  CRLB  [52]  -  The  method  still  has  the  limitations  of  dependence 
on  arbitrary  weights  and  loss  of  directional  information  through  compression  to  a 
scalar,  but  provides  a  variation  on  needing  a  fixed  final  time  by  using  a  range-to- 
go  as  the  independent  variable.  This  is  appropriate  for  systems  that  cannot  turn 
directly  orthogonal  to  the  target,  and  have  a  small  variation  in  range-to-go  rate  (else 
the  measurement  update  interval  becomes  a  function  of  the  path  length  and  direction, 
as  a  fixed  number  of  measurements  must  still  be  declared). 

2.3.3  Trajectory  Optimization  Shortcomings. 

Though  an  extensive  body  of  work  exists  in  the  field  of  trajectory  optimization 
with  a  bearing-only  sensor,  there  are  areas  which  still  need  to  be  addressed.  Regard¬ 
less  of  the  metric  selected,  all  of  the  methods  suffer  from  compression  when  trying 
to  characterize  the  directional  certainty  about  a  point  with  a  scalar.  Second,  there 
is  no  method  of  dual  control  that  does  not  at  some  level  depend  on  an  arbitrary 
weighting  balance  between  control  and  estimation.  Further,  these  methods  require 
pre-declaration  of  some  variables  (final  time,  path  length,  number  of  maneuvers, 
and/or  number  of  measurements)  that  the  solution  is  sensitive  to. 

Perhaps  most  importantly,  there  does  not  exist  a  practical  method,  implementable 
in  real-time,  for  achieving  a  particular  final  covariance  (Frew  did  this  in  a  pure  local¬ 
ization  sense  without  consideration  for  control,  but  exhaustive  search  is  not  feasible 
for  real-time  work  at  this  point).  This  is  a  major  stumbling  block  for  actual  use  of 
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these  methods,  beyond  simulation.  Current  methods  provide  guidance  to  get  “the 
best  estimate  possible  in  a  given  time/path  length/number  of  measurements,”  or 
provide  “more”  information  based  on  a  weighting  scheme  with  current  covariance. 
This  is  not  feasible  for  real-world  applications  which  operate,  even  stochastically,  in 
reference  to  measurable,  physical  limitations. 

In  the  submarine  attack  example,  deviations  from  a  direct  path  to  the  target 
will  increase  the  fidelity  of  a  target  estimate,  but  also  take  precious  time  and  could 
cost  the  first  shot.  Maneuvers  should  be  kept  at  the  minimum  necessary  for  a  valid 
fire-control  solution — physical  requirements  based  on  the  torpedo  capabilities  and 
friendly-fire  clearance  limitations.  For  the  HARM  example,  the  target  estimate  needs 
to  be  of  a  quality  to  ensure  the  desired  effects,  based  on  real  ranges  of  circular-error- 
probable  miss  distance  and  effective  blast  radius.  Maneuvers  beyond  this  deplete 
energy  for  the  critical  end-game  maneuvering.  For  the  sUAS  studied  in  this  work, 
the  physical  drivers  of  the  problem  are  the  ability  of  the  aircraft  to  accurately  reach 
a  commanded  point,  and  the  physical  dimensions  of  the  attachment  apparatus  used 
to  connect  to  the  wire. 
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III.  Problem  Description  and  Modeling 


ONSIDER  the  autonomous  control  of  a  sUAS  for  surveillance  and  other  mis- 
1^  sions.  Completely  autonomous  UAS  control  for  surveillance  missions  is  still 
an  on-the-horizon  capability,  requiring  a  combination  of  several  technologies,  some 
still  relatively  immature.  Decision  making,  mission  definition  and  accomplishment, 
target  identification  and  measurement,  obstacle  avoidance,  and  long-range  communi¬ 
cation  of  surveillance  data  are  not  addressed  here.  The  scope  of  this  dissertation  deals 
with  a  small  part  of  the  overall  mission — energy  harvesting  from  a  power  line.  Short 
range  and  limited  station  times  are  active  constraints  on  the  usefulness  of  our  small 
and  micro-UASs.  Both  could  be  greatly  extended  through  the  ability  to  recharge 
batteries.  Conceptually,  a  small  group  of  sUASs  could  be  sent  for  surveillance  of  the 
same  target.  With  two  recharging  on  a  nearby  power  line  and  a  third  in  the  air, 
continuous  coverage  could  be  provided  without  operator  input. 

The  concept  of  energy  harvesting  through  induction  is  not  new,  and  the  use  of 
power  mats  and  such  for  cordless  devices  is  becoming  commonplace.  The  most  effi¬ 
cient  method  is  to  place  a  clamp  around  a  source,  as  done  with  an  an  inductive  ring 
around  a  spark  plug  wire  for  an  old  timing  light.  Getting  an  inductive  clamp  down 
to  a  light  enough  weight  realistic  for  small  vehicles,  yet  effective  enough  to  charge  in 
a  reasonable  time  without  arcing  problems  is  a  current  topic  of  research  at  Defense 
Research  Associates  (DR A). 


3.1  Segmentation  of  Control  Modes 

The  process  for  landing  on  a  power  line  will  require  several  segments,  where  the 
goals  and  methods  of  control  change  as  milestones  are  accomplished.  The  minimum 
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number  of  control  mode  switches  include  an  acquisition  segment,  where  control  is 
provided  to  reach  a  position  likely  to  pick  up  the  power  line  in  a  sensor,  an  approach 
segment,  where  control  is  determined  by  the  observability  needs  to  accurately  estimate 
the  wire’s  relative  positive  while  guiding  to  an  offset  approach  point,  and  a  flare 
segment,  where  control  is  provided  to  perform  a  maneuver  that  will  safely  attach 
to  the  wire  from  known  flight  conditions  and  offset.  The  concept  is  illustrated  in 


Figure  11 


1.  Identify  Wire, 
Acquire  Angle 


3.  Guide  to  Relative  Approach 
Point,  Achieving  Enough 
Certainty  to  Land 


2.  Optimally  Maneuver  4.  Flare  to  Hang  on  Wire 
for  Range  Observability  /  / 


The  acquisition  segment  is  within  our  current  capability.  It  is  assumed  that  the 
vehicle  has  navigational  awareness  through  GPS,  INS,  optical  flow,  or  some  other 
capacity.  This  includes  having  a  rough  knowledge  of  power  line  locations,  available 
on  local  maps  or  from  imagery.  While  certainly  not  accurate  enough  to  land  with,  this 
is  sufficient  to  find  a  power  line  by  maneuvering  to  a  position  orthogonal  to  the  wire. 
Identification  of  the  line  can  be  accomplished  with  a  feature  extraction  algorithm, 
such  as  a  fast  Hough  transform,  operating  on  sequential  images.  The  images  can 
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be  collected  from  a  device  such  as  simple  webcam,  available  on  many  of  the  smaller 
UASs. 

This  work  addresses  the  approach  segment — beginning  with  an  initial  measure¬ 
ment  of  angle  to  the  wire,  and  ending  at  an  approach  point  with  the  prescribed  states 
necessary  to  begin  a  flare-to-land  maneuver,  such  as  relative  distance,  relative  height, 
heading,  speed,  and  other  requirements  for  specific  systems.  Since  the  approach  point 
is  defined  relative  to  the  power  line’s  true  location,  it  must  be  estimated  to  a  quality 
likely  to  end  in  a  successful  flare  prior  to  arrival. 

The  actual  flare  segment  is  currently  being  investigated  by  several  institutions 
for  fixed-wing  sUASs.  In  [T9],  a  fixed-wing  glider  was  perched  on  a  wire  using  an 
aggressive  flare  maneuver  from  both  2.5-rn  and  1.5-rn  approach  points,  using  full 
information  about  the  location  of  the  wire.  The  approach  point  in  this  work,  xapp, 
was  correspondingly  set  to  2-m. 

Since  the  test  platform  for  the  algorithm  was  a  helicopter  vice  a  fixed-wing  UAS, 
an  aggressive  flare  segment  was  not  required.  The  final  condition  in  the  optimal 
controller  was  simply  set  to  slow  to  a  hover  by  the  time  it  reached  the  approach 
point.  Once  the  final  conditions  are  achieved,  to  include  the  minimum  target  position 
certainty,  the  RTOC  control  mode  is  switched  off,  and  the  helicopter  flies  directly  to  a 
perch  point  underneath  the  last  known  location  of  the  wire,  continuing  to  update  its 
position  until  the  wire  is  no  longer  in  the  field-of-view  of  the  camera.  As  the  vehicle 
approaches  the  perch  point,  it  slows  gently  to  a  stop  and  descends  to  engage  a  hook 
on  top  of  the  vehicle. 
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3.2  Modeling  for  the  Relative  Position  Problem 


Full  modeling  for  control  of  the  real  quadrotor  involves  3-axis  position  and  veloc¬ 
ities,  orientation  angles  and  rates,  engine  states  and  lag  estimates,  control  variables, 
and  many  other  parameters  in  four  reference  frames.  The  necessary  portions  of  the 


quadrotor  and  its  flight  controls  are  described  in  Chapter  VIII  For  consideration  of 
only  the  relative  estimation  problem  and  the  optimal  control  portions  of  the  problem, 
however,  the  model  can  be  greatly  simplified. 

Body  frame  coordinates,  x&  =  (x&,  i/b,  Zb)  G  R3,  are  defined  on  the  quadrotor  with 
the  origin  at  the  center  of  gravity  (eg),  the  positive  x^-axis  direction  pointing  out  of 
the  camera  (referred  to  as  the  “nose”  of  the  vehicle),  x/b- axis  positive  out  of  the  “right 
wing”,  and  z^-axis  positive  up  (non-standard,  left-handed  system  for  readability  of 


later  plots),  as  shown  in  Figure  12 


Though  the  estimation  may  be  performed  in  purely  relative  terms,  reference  to 
the  inertial  frame  must  be  maintained  to  avoid  constraints,  be  they  aerodynamic 
limitations  (maximum  altitude),  physical  considerations  (terrain,  walls),  or  tactical 
limits  (political  borders,  assigned  airspace).  A  navigation  frame  is  defined,  anchored 
inertially,  with  the  x-axis  parallel  to  an  assumed  flat  Earth  and  positive  in  the  shortest 
direction  to  the  power  line  from  the  point  at  which  the  first  measurement  is  received. 
The  y-axis  is  defined  orthogonally,  parallel  to  the  Earth  and  positive  in  the  same 
direction  as  the  7/6- axis  at  initialization  (all  Euler  angles  zero).  The  z-axis  is  again 
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defined  positive  up  (non-standard)  for  convenience.  For  the  actual  flight  test,  the 
origin  of  the  navigation  frame  was  at  the  center  of  the  indoor  flight  test  facility. 

As  detailed  in  Chapter  [TJ  the  power  line  is  modeled  as  horizontal,  with  the  angle 
along  the  wire  unobservable.  This  leads  to  a  planar  problem,  with  maneuvering  in  the 
vertical  to  increase  observability.  For  simplicity,  all  reference  to  the  y- axis  is  omitted 
from  mention,  except  when  necessary  in  the  discussion  of  flight  control.  During  flight 
test,  the  vehicle  is  directed  to  y  —  0  prior  to  the  first  measurement,  and  is  regulated  to 
zero  during  the  run.  The  inertial  position  coordinate  vector,  x  G  M2,  is  then  defined 
as: 


x(t) 


x{t) 


(8) 


Velocity  is  likewise  defined  in  the  planar  navigation  frame: 


v(t) 


Vx{t) 
Vz{t ) 


cht{t) 

dt 


(9) 


An  upper  total  velocity  limit,  v2  +  v\  <  v\ax ,  was  imposed  (no  minimum  speed 
required  for  a  helicopter),  but  in  the  manner  controls  were  actually  applied  to  the 
quadrotor,  individual  limitations  of  \vx\  <  vXmax  and  \vz\  <  vZrnax  became  more  re¬ 
strictive. 

The  true  target  coordinates  are  ( xt ,  zt ),  and  estimates  are  denoted  with  the  hat 
symbol,  as  in  Xt ■  For  notational  convenience,  a  vector  of  relative  coordinates  between 


the  target  and  the  vehicle  is  defined  using  the  convention  shown  in  Figure  13 


III 

ho' 

X 

1 

ho'' 

= 

xt  -  x(t) 

1 

0s 

J- 

- 1 

HO^ 

1 

h? 

(10) 
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Figure  13.  Relative  Cartesian  Formulation 


The  measurement  angle,  /3,  is  received  by  a  camera  mounted  in  the  nose  of  the 
quadrotor,  and  defined  positive  up  from  the  horizon.  The  camera  is  fixed  in  position 
and  orientation  relative  to  the  eg  of  the  vehicle.  With  no  required  lateral  motion,  the 
bank  angle,  0,  and  heading  angle,  0  are  regulated  to  zero.  The  measurement  angle  is 
then  considered  to  be  vertical  (or  corrected  to  vertical)  from  the  level  inertial  frame: 


h  [x(t) ]  =  (3(t )  =  tan 


-i 


zr(t) 

xt(t) 


(11) 


The  function  symbol  tan-1  refers  to  the  full  quadrant  arctangent  throughout  this 
dissertation.  It  should  be  noted  that  the  measurement  angle  is  a  combination  of  the 
image  angle,  /3jma£;e,  produced  from  a  pixel  count  in  a  known  FOV,  with  the  deck 


pitch  angle,  6 ,  as  shown  in  Figure  14  Because  the  calculation  of  the  image  angle 


causes  some  delay,  it  is  critical  that  the  images  be  time  tagged  and  correlated  with  a 
short  history  of  pitch  angle  measurements. 


m  =  p  image  (t)  +  9{t) 


(12) 


Camera  Inertial  Pitch  Angle 


Position 

(nose) 


Figure  14.  Correction  of  Deck  Pitch  Angle  for  Inertial  Measurement 


For  estimation  purposes,  (3  is  measured  at  discrete  times,  tk  €  [t0jtf],  and  is 
modeled  as  an  independent  random  variable: 


&  =  h(xfc)  +  rjk 


(13) 


where  {^fe}^=1  is  a  zero- mean,  Gaussian,  white  noise  sequence  with  a  constant  covari¬ 
ance: 


E[rjk\  =  0 

E[rikvj]  =  Rfaj 


(14) 


with  5kj  representing  the  Kronecker  delta  function.  The  added  noise  models  the 
combined  uncertainty  in  the  measurement  from  errors  in  the  line  detection  algorithm 
and  errors  in  the  estimate  of  the  current  pitch  angle. 

For  actual  implementation,  it  is  important  to  consider  the  fact  that  the  eg  of 
the  vehicle  is  not  likely  to  be  collocated  with  the  bearing  sensor.  In  this  case,  the 
optimal  trajectory  planning  is  really  a  sensor  positioning  algorithm  and  the  optimal 
path  solved  for  is  really  the  optimal  path  of  the  camera.  If  significant,  the  effects  of 
the  transformation  must  be  considered  on  the  constraints  and  the  control,  with  the 
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appropriate  transformation.  In  this  case,  assuming  a  fixed  camera  lever  arm  in  the 


body  frame,  (rcanix ,  rcamz ) ,  a  direction  cosine  matrix  (DCM)  is  used: 


'X'camb  (^) 

^ camx 

Zcamb  (t) 

Zb(t)  +  rcamz 

(15) 


%cam  (t') 

x{t) 

+ 

Zcamky) 

z(t) 

cos  0(f)  —sin  0(f) 
sin  0(f)  cos  0(f) 


(16) 


Control  is  modeled  after  the  actual  quadrotor,  which  uses  the  advantages  of  a 
helicopter  to  decouple  vertical  and  horizontal  components: 


u(f) 


Ux{t) 
Uz(t ) 


dtv{t ) 
dt 


(17) 


limited  by  \ux\  <  and  <  uz  <  (^)  ,  with  gravity  causing  a 

difference  in  vertical  acceleration  capability.  This  model  is  limited  by  two  factors. 
A  helicopter  near  maximum  performance  cannot  accelerate  upward  and  forward  at 
maximum  rates  simultaneously.  In  addition,  the  real  equations  of  motion  have  more 
lag  caused  by  additional  integration  steps  in  horizontal  acceleration.  The  true  control 
signal  is  a  differential  RPM  on  the  motors.  The  corresponding  lift  difference  changes 
the  pitch  or  bank  angle,  which  then  causes  horizontal  acceleration.  For  the  slow 
speeds  and  very  low  bank  angles  of  the  quadrotor  in  the  indoor  flight  test  facility, 
however,  this  model  was  sufficient  for  outer  loop  trajectory  planning.  For  an  example 
of  backing  out  controls  down  to  the  servo  level  from  optimal  trajectories,  see  urn 
For  actual  propagation  and  use  in  the  own-ship  position  Kalman  Filter,  the  ve¬ 
locity  and  acceleration  were  assumed  constant  over  a  time  step,  and  the  discrete-time 
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state  equation  was  used: 


x 


(I<F) 
k+ 1 


*£fc+l 
Zk-\- 1 

VXk+l 


V 


%k+ 1 


I2  I2At 

O2  I2 


02 

I2At 


ufc  +  wfc 


(18) 


with  E  [wfe]  =  0  and  E  [w*wf]  = 


3.3  Transformation  to  Polar  Coordinates 

The  final  flight  test  version  of  the  software  developed  in  this  project  was  imple¬ 
mented  in  the  Cartesian  frame.  Much  of  the  research,  however,  was  accomplished 
using  a  polar  coordinate  transformation.  This  is  still  recommended  for  some  sce¬ 
narios,  as  will  be  discussed  in  Chapter  |IV|  In  this  dissertation,  the  polar  coordinate 
system  is  non-standard,  with  the  origin  at  the  estimated  target  position,  as  shown  in 


Figure  15,  defining  p(t)  positive  for  the  current  range,  and  the  polarity  of  (3  opposite 


of  the  tradi  ' 


Ceiling  Limit 


/:liL 


V- . 

% 


Estimated  Target 
Position 


Floor  Limit 


Figure  15.  Polar  Formulation 
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This  formulation  allows  several  advantages,  and  is  recommended  for  similar  re¬ 
search  that  has  fewer  position  constraints,  such  as  the  submarine  problem,  and  sensor 
systems  capable  of  using  angular  rate,  /3,  in  the  cost  function  or  constraints.  The  ad¬ 
vantage  to  the  Cartesian  system  is  fast  propagation  of  the  linear  dynamics,  at  the 
cost  of  a  non-linear  measurement  function.  The  polar  system,  defined  as: 

p(t) 

m 

has  a  linear  measurement  function: 


Hy  =01  y  (t)  (20) 

The  linear  measurement  function  will  aid  in  accurate  measurement  updates  to  the 
target  estimate,  but  typically  at  the  cost  of  a  non-linear  dynamics  function.  However, 
if  control  is  applied  in  the  form  of  radial  acceleration,  pit),  defined  as  positive  away 
from  the  target,  and  tangential  acceleration,  $(t),  defined  positive  clockwise: 

Pit) 

m 

Pit) 
fi{t) 

The  dynamics  can  then  be  represented  as  fully  linear  and  time  invariant: 

y  tKF\t)=  °2  12  y<RF>(«)  +  °2  uy(i)  (22) 

02  02  I2 
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Approaching  the  model  in  this  manner  pushes  all  of  the  non-linearities  into  the 
determination  of  the  initial  conditions  for  every  epoch  of  the  path  planner,  and  into 
the  constraints.  Through  the  constraints,  the  control  of  the  helicopter  can  be  appro¬ 


priately  scheduled.  Noting  that  v  =  —dxr/dt,  the  initial  conditions  can  be  found 


with: 


y{KF)(t0 ) 


y/Xr(to)  +  Z*(t0)  ™, 

rad 


tan-1^#! 

Sr  (*o) 

-Xr(to)vx(to)-Zr(to)vz(t0)  / 
y/ X%(to)+Z*(to) 

grho  )rr(U))  —  'rr  (to)  (fp) 

xl(to)+z$(to) 


rad/ s 


(23) 


Position  constraints  (floor,  ceiling)  can  be  applied  with  the  simple  transformation: 


(yZmin  Zt)  <  p(t)  sin  fd(t)  <  (  Zmax  Zt) 


(24) 


Speed  and  acceleration  constraints  are  developed  with  (dropping  time  depen¬ 
dency)  : 


zr  =  p  sin  f3 

=>■  zr  —  $p  cos  j3  +  psm/3  (25) 

=>  vz  —  —  $pcos/3  —  p  sin  fd 

Making  total  squared  velocity: 


xr  =  p  cos  fd 

=>  xr  =  —ftpsinfd  +  p  cos  fd 
vx  —  (dp  sin  fd  —  p  cos  [3 


v2  +  vz2  =  jd2 p 2  sin2  (d  —  2  fdpp  sin  fd  cos  fd  +  p2  cos2  fd  +  fd2p 2  cos2  (d 

+  2 /dpp  cos  fd  sin  fd  +  p2  sin2  fd 


=  p2  +  m2 


(26) 
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constrained  with: 


(27) 


»mm  <P2  +  (Pf>)2  <  « 


2 

max 


The  acceleration  limitations  on  the  vehicle  are  similarly  treated.  The  horizontal 
acceleration  capability  of  the  quadrotor  becomes  limited  by: 


vx  =  /32p  cos  [3  +  \j3p  +  p/3j  sin  (3  +  ftp  sin  (3  —  p  cos  (3 
'  p[3  +  2$pj  sin  f3  +  (j32p  -  pj  cos/3  < 


The  vertical  limitation  is  then  transformed  to: 


(28) 

(29) 


vz  =  (32p  sin  f3  —  (f3p  +  /3p)  cos  (3  —  j3 p  cos  (3  —  p  sin  (3 

i)CoS/3< 


)  £  (ri2t>  -  /> )  •-•in  /'J  +  (  -:i() 

/  min 


(30) 

(31) 


It  is  stressed  that  this  method  can  be  used  for  trajectory  optimization  even  though 
the  actual  range  to  the  target  is  not  known.  The  current  navigation  estimate  is 
provided  to  the  path  planner  as  if  it  were  the  actual  target  location.  The  constraints 
are  valid  because  they  are  defined  relative  to  that  point  in  space,  whether  it  ends 
up  being  the  actual  target  location  or  not.  In  implementation,  it  was  found  that 
the  linear  measurement  function  was  a  strength  for  the  estimation  filter,  but  the 
significant  non-linearities  in  the  path  constraints  had  potential  to  slow  down  the 
optimization  (slightly).  In  an  attempt  to  get  the  most  from  both  worlds,  a  Hybrid 


EKF  is  developed  in  Chapter  IV  that  can  be  used  in  some  scenarios,  as  well  as  the 


UKF  that  was  used  in  the  final  power  line  landing  flight  tests  that  validated  the 
complete  system. 
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IV.  Bearing-only  Estimation 


ANGE  estimation  is  clearly  the  core  of  the  bearing-only  analysis  problem  in 
%  this  dissertation,  and  the  concepts  of  range  observability  are  central  to  the 
trajectory  optimization  problem  as  well.  The  fundamental  non-linearity  of  the  system 
has  made  this  a  classic  relative  estimation  problem. 

Historically,  triangulation  has  been  accomplished  by  solving  a  system  of  equations 
generated  from  a  point-slope  equation  taken  at  each  measurement  position,  with  the 
target  coordinates  as  unknowns,  or  by  generating  equations  with  the  law  of  sines, 
and  using  the  collection  of  ranges  at  each  measurement  as  the  unknowns.  With 
two  measurements,  the  answer  for  the  estimate  is  exact  (wrong,  excepting  perfect 
measurements,  but  exact).  With  more  than  two  measurements,  the  solution  is  over 
determined.  A  matrix  of  equations  is  formed,  and  the  estimate  error  is  minimized  (in 
the  2-norm  sense)  with  a  pseudo-inverse  following  the  linear  least  squares  method. 

For  many  on-line  applications,  at  least  for  linear  systems,  the  Kalman  Filter  has 
become  the  industry  standard — propagating  a  system  forward  based  on  known  con¬ 
trols  and  modeled  dynamics,  estimating  what  the  next  measurement  will  be  at  that 
state,  and  applying  a  portion  of  the  residual  difference  between  the  actual  and  ex¬ 
pected  measurements  based  on  an  optimal  Kalman  gain.  Again,  proper  selection  of 
the  gain  minimizes  the  errors  in  a  least  squares  sense. 


For  non-linear  systems,  as  mentioned  in  Chapter  III  the  coordinate  representa¬ 
tion  selected  can  impact  the  ability  to  accurately  estimate  relative  position.  For  the 
bearing-only  estimation  problem,  sensor  measurements  can  be  made  linear  in  a  polar 
coordinate  system,  but  the  propagation  of  the  system  is  linear  in  the  Cartesian  frame. 
Reference  to  a  Cartesian  navigation  frame  must  be  maintained  for  considerations  such 
as  inertial  own-ship  position  estimation  and  ground  avoidance,  but  understanding  of 
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the  polar  reference  frame  is  also  required  to  maintain  the  target  within  the  camera 
FOV  angle  limits. 

The  Cartesian-polar  conversion  problem  is  almost  ubiquitous  in  tracking  and  nav¬ 
igation  applications,  and  many  solution  options  have  been  created  to  minimize  the 
effects  of  the  non-linearity.  The  Extended  Kalman  Filter  (EKF)  pT]  is  a  common 
approach,  linearizing  the  measurement  function  by  evaluating  its  Jacobian  at  the 
current  estimated  state,  allowing  the  linear  Kalman  filter  equations  to  be  used.  The 
EKF  has  significant  limitations,  however.  Linearization  of  the  measurement  function 
is  only  as  good  as  the  current  estimate,  and  errors  will  not  only  cause  the  state  up¬ 
date  to  be  biased,  but  will  result  in  over-confidence  in  the  covariance  matrix.  This 
ill-conditioning  can  cause  the  uncertainty  estimates  to  collapse  prematurely  around 
bad  state  estimates,  leading  to  instability  of  the  filter.  This  is  particularly  prevalent 
when  the  degree  of  non-linearity  is  high,  or  when  the  initial  estimates  for  mean  and 
covariance  are  significantly  off  {TJ. 

In  the  submarine  context,  the  Modified  Polar  form  introduced  in  [1]  assists  with 
this,  especially  at  long  ranges  with  low  bearing  rates.  The  drawback  is  that  the  Li¬ 
ter  still  must  deal  with  conversion  to  and  from  Cartesian  coordinates  to  avoid  the 
real-world  navigation  and  dynamic  constraints.  Other  options  include  least  squares 
Liters  ung,  maximum-likelihood  techniques  [32],  particle  Liters  [63],  Gauss  meth¬ 
ods  [32>] ,  pseudo-linear  trackers  [51],  and  many  other  debiasing  techniques  [69]. 

For  this  research,  two  main  estimation  Liters  were  used — a  Hybrid  Extended 
Kalman  Filter  and  an  Unscented  Kalman  Filter.  Both  Liters  were  found  to  be  ef¬ 
fective,  with  little  diLerence  in  actual  estimation  performance.  The  ability  to  get  an 
unbiased  mean  for  a  small  amount  of  additional  complexity,  and  the  required  shape 
of  the  Lnal  covariance  ellipsoid  were  the  primary  drivers  of  the  design  decision  to  use 
the  unscented  Liter.  The  UKF,  in  the  form  implemented,  provides  the  expected  Lnal 
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covariance  estimates  in  terms  of  Pxx  and  Pzz  (the  respective  diagonal  elements  of  the 
covariance  matrix),  without  using  an  undesirable  extra  non-linear  conversion.  For  the 
particular  implementation  of  the  quadrotor,  the  shape  of  the  hook  used  to  attach  to 
the  wire  necessitated  that  the  wire’s  position  be  known  to  a  particular  level  in  the 
z-axis  direction  to  enter  the  “mouth,”  and  a  particular  level  in  the  x-axis  direction  to 
know  when  to  stop,  and  when  to  descend  (see  Figure  [l6|.  This  made  the  UKF  more 
desirable. 


Figure  16.  Quadrotor  Hook  Design 


For  cases  where  uncertainty  in  terms  of  range  and  angle  is  important,  such  as 
would  be  for  the  likely  sUAS  design  of  a  device  with  a  conical  “mouth”  to  attach  to 
a  power  line,  the  HEKF  should  be  considered.  For  long  range  engagements  with  a 
slow  bearing-rate,  the  modified  polar  form  is  recommended. 


4.1  The  Hybrid  EKF 

The  HEKF  is  a  variant  of  the  EKF  based  on  [1] ,  but  not  using  the  modified  polar 
form.  It  is  a  mid-point  between  using  an  EKF  defined  purely  in  either  the  Cartesian 
or  polar  frames,  and  variants  of  it  have  been  used  in  cases  such  as  this  where  the 
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motion  of  the  vehicle  will  be  provided  in  one  frame,  but  the  measurement  in  another. 
The  effect  of  the  non-linearity  is  not  magically  bypassed — a  transformation  between 
the  forms  will  still  be  made  based  on  a  faulty  angle  estimate.  However,  by  applying 
the  propagations  that  we  know  in  inertial  space,  the  measurements  we  know  in  polar 
space,  and  by  tracking  state  error  from  a  nominal  condition  vice  the  actual  states,  we 
can  minimize  the  filter  errors. 

As  the  nominal  trajectory  will  be  provided  by  the  path  planner,  there  is  no  need  to 
track  the  velocity  or  acceleration  in  the  filter.  The  uncertainty  in  the  position  errors 
are  assumed  to  have  reached  a  steady-state  (additive  covariance),  and  the  size  of  the 
errors  are  assumed  to  be  small  in  relation  to  the  errors  in  the  angle  measurements. 
Only  the  two  relative  states  are  then  required,  as  defined  in  Equations  [T9]  and  20 


on 


page  48,  and  related  by  the  one-to-one  transformations: 


xr(t)  =  sx  [y(t)j  = 

y  (t)  =  Sy  M*)]  = 


Vi(t)  cos  y2(t) 

2/i  (t)  sin  2/2  (i) 


y/xl{t)  +  X%{t) 


tan  1 


X2  (t) 

Xl(t) 


(32) 


(33) 


Assuming  that  the  vehicle  will  move  along  a  nominal  path,  we  can  add  changes 
in  Cartesian  position  over  a  time  step,  q(t,t0),  for  a  discrete  standard  form  of  the 
dynamics: 


Xr(t)  =  A(f,  to)xr(to)  +  s(t,t0) 

=  A(t,to)sa.  [y(fo)]  +  ?(Mo)  (34) 
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For  the  linear,  Cartesian  case,  A (£,  t0)  is  reduced  to  identity.  Substituting  Equation  34 
into  Equation  [33]  yields  the  propagation  equation  for  our  states  in  polar  form: 


y (t)  =  s y  [A(t,  t0)sx  [y  (t0)]  +  ?(*,  t0)} 
=  s[y(t0y,t,t0\ 


(35) 


Following  the  Extended  Kalman  Filter  derivation  EZJ.  and  using  the  X\k\k-i  to 
indicate  the  hrst  element  of  xr  at  time  tk  using  all  available  measurements  through 
time  tk~i,  we  may  express  this  in  a  discrete  propagation: 


[Afc^-iSj;  [yfc— i|fc— i]  +  Sk,k- 1] 

\J  (Sa;1 

tan-1 

[y*!— i|*r— i]  +  ?ifc,fc_i)  +  (sx 2  [yfc— i|fc— i]  +  ^2fe,fe— i) 
s*a[y(fc— l|fc— i)]+e(fc>fc— 1)1 

Sasi  [y(fc — i|fc — i)H- 

-?i(fc,fc-i)  J 

\Jxik\b 

tan-1 

—  1  +  X2k\k-1 

x2k\k  —  l 
xlk\k  —  l 

(36) 

and  may  formulate  a  matrix  of  partial  derivatives  evaluated  along  the  nominal  tra¬ 
jectory: 


S 


k,k—  1 


<9s  [yk-i\k-iitk,tk~i] 
dyk-i\k-i 


(37) 


For  ease  of  calculation,  observe  that: 


^rk\k—  1  A-k,k—l^-rk—l\k—l  4“  1 


2/ifc— i|fc— i  cos  2/2fc_i|fc_i  +  Sik,k-i 
2/lfc— l|fc— 1  sin  2/2fc— 1|A:— 1  +  ^2k,k-l 


(38) 
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COS  U2k-l\k-l  —yik-l\k-l  sin  Z/2fc-i|fc_i 
sin  2/2fc— i|fc— 1  2/ife— i|fe— 1  cos  2/2fc— i|fc— 1 

cos  2/2fc_l|fc_l  —X2k-\\k-\ 
sin  2/2fc— i|fc— l  ^lfc-ilfc-i 


<9x 


<9yfc-i|fc_ 


i 


(39) 


Allowing  the  partials  of  the  transformation  to  be  formed: 


<9si  |yfc-i|fe-i;4,4-i_ 


\jXlk\k-l  +  X2fc|fc-l/ 


2a: 


(9a: 


ifc|fc— i 


ifc|fc— i 


^2/ifc— i|fc— l 


^lfelfc-i  cos  J/2fc — i|fc — i  +  x2k\k-i  sin  2/2fc_i|fc— i 


\JXlk\k-l  + 


+  2a:: 


'2fc|fc— 1 


^2fe|fe-l 
^2/ifc— i|fc— i 


(40) 


<9si  [yfc_i|fc_i;4,4-i_ 

<92/2fc_l|fc— 1 


dx 


ifc|fc— i 


^felfe-l^lfe-llfc-l  —  Xik\k-lx2k-l\k-l 
\JXlk\k-l  +  X2k\k-1 


) 


dx 


2k\k-l 


2xik\k-i  t: -  +  2a:2fc|fc_i  w - 

V  dV2k-i\k-i  J  \  dy2k-i\k-i 


i|fc- 


(41) 


<9s2  [yfc— i|fc— i ;  tk,  tk-i] 
dyik-i\k-i 


1  + 


x2k\k  —  1 
fc|  A:  —  1 


_  ^ifc|fc-i  sin 2/2fe— i|fc— l 


xn|fc-i 
■  ^2fc|fc-l  COS  2/2fc — l|fc — 1 


x 


lk\k-l  +  X2k\k-1 


(42) 
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1 


ds2  [yfc_i|fe_i;4,4-i] 

dy2k-i\k-i 


1  + 


1  2 


d%2k\k  —  l  dx\k\k  —  1 

lfclfe-l%2fc l|fc l  2fc|fc-i  07/2 fe  1 1 fc  1 


a: 


lk\k-l 


*^2fc|fc  — 1 
*^1  fc|fc  —  1 

Xlk\k-lXlk-l\k-l  +  a;2fc|fc-1^2fc-l|fc-l 

/y»2  I  ry  2 

xl/c|fc-l  ^  x2fc|fc-l 


(43) 


Making  the  complete  partial  derivative  matrix  in  a  form  that  will  be  used  for  discrete 
propagation  of  the  covariance: 


Sfc,fc— i 


^lfc|fc— 1  COS4/2fc— i|fc  — 1+Z2fc|fc  — 1  sin^2fc-l|fe  — l  Z2fe|fc-l3~lfc_i|fc_i—  X\k\k_1X2k-l\k-l 


2 

2fc|fe — 1 


xlfc|fc-i  sin^2fc_i|A!_i—  a^2fc|fc— 1  cos;;2fc--i|it,-i  x\k\k_1x\k_1\k_1+X2k\k-lx2k-l\k-l 

^  i  r2 i r2 

— 1  '  *^2fc|fc  — 1  xlfc|fc  — 1  '  U/2fc|fc  — 1 


(44) 


4- 1-1  Hybrid  Filter  Algorithm. 


To  assemble  the  filter,  the  typical  assumption  of  a  Gaussian  distribution  of  mea¬ 
surement  noise  that  is  uncorrelated  in  time  is  accepted,  and  is  reasonable  for  this 
scenario.  The  filter  must  be  initialized  with  an  initial  mean,  yo>  and  covariance,  Pyo, 
using  the  most  likely  pickup  bearing  (the  mostly  likely  initial  angle  based  on  the  ac¬ 
quisition  segment  profile),  and  the  most  likely  pickup  range,  based  on  analysis  of  the 
true  sensor  performance.  The  typical  EKF  non-linear  integration  for  the  propagation 
of  the  state  estimate  is  replaced  by  simply  applying  the  inertial  change  in  state  for 
one  time  step  from  the  semi-discrete  optimal  path  of  the  trajectory  planner,  x* : 


<?(4,4-i)  =  -  x*(4_i) 


(45) 


The  state  is  then  advanced  with  Equation [34j  and  converted  back  to  polar  coordinates 


with  Equation  33  The  covariance  is  propagated  in  polar  form  as  well,  with: 


P*|*-l  ^k,k— lP/c—  l\k—  l^k,k—  1 


(46) 
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Note  that  no  process  noise  was  added  to  the  state  or  covariance  propagation  equa¬ 
tions,  based  on  the  assumption  that  the  inertial  vehicle  position  estimate  had  reached 
steady-state.  This  means  that,  with  a  static  target,  the  target  position  estimate  un¬ 


certainty  does  not  grow  between  measurement  updates. 

Measurements  are  modeled  as  in  Equation  |13  on  page  45  but  replacing  the  mea¬ 
surement  function  with  the  polar  form  from  Equation  [20} 


6=  =  Hyyfc  +  r]k 


(47) 


When  measurements  become  available,  the  system  estimate  and  error  uncertainty 
can  now  be  updated  with  the  common  linear  Kalman  Filter  equations: 


Kfc 

y  k\k 

p 

ryfc|fc 


>  H 

yfc|fc-iAJ-y 


T 


HyPyfc|fc_iHy  +  R 


-1 


y k\k— i  T  K k  Hyyq/j—iJ 


Pyfc|fc-i  ^-fc^yPyfc|fc-i 


(48) 


This  form  of  the  HEKF  was  used  for  much  of  the  build-up  research  prior  to  the 
hnal  flight  test  with  the  actual  wire,  and  it  remains  a  potential  option  for  others 
following  with  similar  scenarios.  In  addition,  in  Chapter  [Vj  the  information  states 
and  their  dynamics  are  developed  from  the  Fisher  Information  Matrix  using  the  same 
fundamental  principles  used  here  for  the  EKF.  For  the  final  flight  test  however,  the 
desire  to  represent  the  final  error  covariance  in  the  Cartesian  frame,  and  the  desire 
to  avoid  a  potential  bias  from  the  estimated  mean  drove  the  decision  to  use  the 
Unscented  Transformation. 


58 


4.2  Unscented  Kalman  Filter 


The  UKF  was  used  for  target  estimation  for  the  final  quadrotor  flight  test  re¬ 


sults  presented  in  Chaper  IX  [59j.  As  in  an  EKF,  the  target  position  estimate  and 


the  measurements  are  characterized  with  probability  density  functions  (pdfs),  in  this 
case  Gaussian,  and  represented  by  the  first  two  moments  of  the  state  (mean  and 
covariance).  The  propagation  and  update  steps  are  again  considered  time  invariant 
Markovian  processes,  allowing  recursive  calculations  at  each  time  step  to  perform 
the  non-linear  transformations  of  the  pdf.  These  calculations  are  referred  to  as  “Un¬ 
scented  Transformations,”  and  when  implemented  with  the  propagation  and  measure¬ 
ment  steps  to  perform  estimation,  the  algorithm  is  dubbed  the  Unscented  Kalman 
Filter,  or  sometimes  the  sigma-point  Kalman  Filter  (SPKF).  The  UT  is  based  on  the 
fact  that  it  is  easier  to  approximate  a  probability  distribution  than  an  arbitrary  non¬ 
linear  transformation.  There  are  several  variants  that  can  be  optimized  for  different 
applications,  varying  such  factors  as  the  selection  of  the  sigma-points  within  the  nec¬ 
essary  conditions,  choosing  the  regression  weights,  and  performing  the  transformation 
to  different  orders  of  accuracy.  For  this  research,  propagation  was  performed  again 
with  the  linear  transformation  using  Equations  [34]  and  [45}  The  measurement  update 
was  performed  with  the  following  algorithm,  taken  from  [59]: 


1.  Sigma-points,  X  e  Mnx>d2rix+b,  are  selected  for  the  nx  states  in  a  manner  that 
maintains,  for  the  set,  the  mean  and  covariance  of  the  current  distribution  prior 
to  the  measurement  update,  x“  and  P)7: 


=  40)~  +  (dKT+ypf). 
=  40)~  -  (Vk + A)  p*) . 


i  =  1, . . .  ,nx 
j  =  nx  +  1, . . . ,  2  nx 


(49) 
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The  symbol  represents  the  ith  column  of  the  matrix  square  root,  ob¬ 

tained  with  the  Cholesky  decomposition. 


2.  Each  state  vector  sigma-point  is  transformed  into  the  measurement  space  through 


the  observation  function,  Equation  11,  with  the  appropriate  element  substitu¬ 
tions  for  xr  and  zr: 


Z 


(0- 


=  h 


X, 


W- 


i  =  0, . . . ,  2nx 


(50) 


3.  The  statistics  of  the  projected  sigma-points  are  calculated  for  the  estimated 
measurements.  The  mean  is  found  with  a  weighted  sum: 


V  2T1t; 


Zl.  = 


^-0=0 


W^Zp' 


(51) 


where  the  weights  are  determined  with: 


=  X/(nx  +  A) 
Wm  =  1/ [2  (nx  +  A)] 


i  =  1, 


(52) 


with  scaling  parameter  A  =  a2  (nx  +  k)  —  nx  to  meet  the  necessary  condition  for 
an  unbiased  mean,  Y^=o  =  1.  The  gain  on  the  sigma-point  spread,  loosely 
speaking,  was  set  at  a  =  0.001,  and  the  tuning  parameter  was  set  at  k  —  0. 
As  the  noise  on  the  variables  is  independent,  the  variance  may  be  additively 
applied,  calculating  the  covariance  and  cross  correlation  with: 


ZZk 


=  E£Wcw  (zk 


r(0 

r(i) 


(»)- 


z. 


(0- 


+  Rk 


(0- 


z, 


(0- 


(53) 
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using  the  covariance  weights: 


1FC(0)  =  X/(nx  +  A)  +  (1  -  a2  +  //) 

(54) 

Wc]  =  1/  [2  (nx  +  A)]  i  =  1, . . . ,  2  nx 

For  this  Gaussian  distribution,  the  tuning  parameter  was  set  optimally  at  fi  —  2. 
4.  A  weighting  gain  is  then  calculated: 


KuKFk 


Pizfc(Pzzk) 


(55) 


and  applied  to  project  the  appropriate  residual  error  onto  the  mean  prediction 
and  update  the  covariance: 

* rk  =  X"  +  K UKFk  (z k  -  Z^) 

Pfc  =  Pfc  —  ^UKFkP  zzJ^UKFk 

During  the  flight  test,  the  UKF  and  the  trajectory  planner  were  run  consecutively. 
As  multiple  measurements  often  became  available  while  the  trajectory  planner  was 
calculating  (A tmeas  =  0.33),  all  new  measurements  were  processed  in  batch  at  each 
iteration. 


(56) 
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V.  Simultaneous  Solution  of  the  Optimal  Control  and 

Estimation  Problems 

IMULTANEOUS  solution  of  the  optimal  control  problem  and  the  optimal  es- 
^  /  timation  problem  requires  breaking  down  the  fundamental  observability  re¬ 
quirements  of  an  estimator  into  elements  that  can  inform  an  optimal  control  solver 
how  to  move  to  make  the  estimate  better,  in  the  midst  of  some  control  task.  Exam¬ 
ples  of  initial  work  in  this  direction  were  provided  in  Chapter  [TT|  under  the  umbrella 
of  localization.  Localization  techniques  find  a  scalar  metric  to  assess  estimate  qual¬ 
ity,  and  seek  the  path  that  will  optimize  that  metric  in  a  given  time  or  number  of 
measurements.  As  has  been  shown,  most  of  the  work  in  localization  is  in  the  area 
of  finding  the  most  desirable  performance  index,  since  compressing  the  necessarily 
multi-dimensional  knowledge  of  a  target’s  position  into  a  scalar  results  in  a  loss  of 
directional  information  that  can  be  problematic. 

Dual  control  concepts  were  also  introduced  that  broaden  this  effort.  The  funda¬ 
mental  localization  techniques  remain  unchanged,  but  a  control  desire  is  added  to  the 
performance  index  with  the  information  metric.  Basic  methods  include  separating 
the  efforts  into  different  dimensions  and  treating  them  independently.  More  compre¬ 
hensive  efforts  typically  pit  the  contending  desires  of  control  and  estimation  against 
one  another — applying  a  cost  functional  element  on  control  that  regulates  the  states 
to  a  particular  path,  and  another  cost  related  to  estimation  quality  that  pushes  the 
states  away  from  that  path  in  an  effort  to  increase  observability. 

Many  of  the  same  limitations  from  localization  exist  for  dual  control.  The  direc¬ 
tional  information  is  still  compressed  to  a  scalar  cost  function,  and  researchers  have 
still  relied  on  a  fixed  final  time  (at  times  indirectly).  Of  further  concern  for  dual 
control  is  the  question  of  how  to  determine  the  set  of  weights  that  balance  how  much 
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effort  should  go  to  each  desire,  typically  dealt  with  by  using  an  arbitrary  function 
of  the  current  estimate  uncertainty.  In  the  end,  the  values  set  for  the  weights  will 
determine  the  overall  level  of  certainty  that  the  system  has  at  the  end  of  the  path, 
which  may  or  may  not  meet  the  physical  needs  of  the  system. 

Each  of  these  limitations  of  current  methods  needs  to  be  addressed  in  turn,  starting 
with  the  scalar  cost  function  (determinant,  trace,  etc.).  Regardless  of  the  metric 
selected,  all  of  them  attempt  to  encapsulate  directional  information  contained  within 
the  Fisher  Information  Matrix. 


5.1  Development  of  the  Fisher  Information  Matrix  from  the  Cramer-Rao 
Lower  Bound 

The  concept  of  Fisher  Information  is  a  byproduct  of  the  development  of  the 
Cramer-Rao  Lower  Bound,  commonly  used  in  estimation,  the  derivation  of  which 
is  taken  from  mu-  Fisher  Information  is  fundamentally  tied  to  the  concept  of  ob¬ 
servability  in  the  framework  of  estimation  theory.  If  a  measurement  is  treated  as  a 
random  variable,  Z,  with  (  being  a  sample  of  that  variable,  and  the  measurement  is 
dependent  on  the  state,  x,  treated  as  an  unknown  but  deterministic  parameter,  then 
a  likelihood  function,  p(Z;  x)  would  describe  the  probability  of  receiving  a  particular 
(  given  a  known  x.  Plotting  p(Z\  x)  gives  insight  into  the  observability  of  x  through 
the  measurement  (.  If  the  plot  showed  a  low  variance  (a  tight  peak),  then  there  is  a 
strong  ability  to  estimate  x  with  the  measurement  It  could  be  said  that  (  relates  a 
good  deal  of  information  about  x,  or  that  x  is  highly  observable.  Note  that  the  ability 
to  estimate  x  is  dependent  on  the  collection  of  measurements.  This  is  characterized 
by  the  FIM,  which  is  developed  from  the  definition  of  an  unbiased  observer,  which 
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states  that  the  error  of  an  estimate  conditioned  on  a  particular  state  will  be  zero: 

/OO 

[x(C)  *x]p(C;x)  d(  =  0  (57) 

-OO 

This  must  be  true  for  all  values  of  x,  therefore: 

Jx  /  [x(C)  -x]p(C;x)  d(  =  0  (58) 

Assuming  that  dp( £;  x) /dx  exists  and  is  absolutely  integrable,  the  partial  is  taken 
inside  the  integral  and  the  chain  rule  is  applied: 

-  J°°  p((;  x)  d(  +  J°°  (S(0  -  x)  d(  =  0  (59) 

Note  that  the  measurement  is  assumed  to  be  Gaussian,  with  the  associated  expo¬ 
nential  distribution.  Therefore: 


<9p(C;x) 

<9x 


p(  C;x) 


ainp(C;x) 

<9x 


(60) 


The  furthest  right  partial  derivative  is  referred  to  as  the  score.  Also  note  that  by 
definition  of  a  pdf: 


p«;x)  (i<  =  1 


(61) 


Equation  59  then  reduces  to: 


(  dhip(C; 

V  <9x 


—p(  C;x)  [x(C) 


d  C 


1 


(62) 


(  ghip(C;x) 

V  <9x 


[p(C;  x)]1/2^  ^[p(C;  x)]1/2  [x(C) 


l 


(63) 
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The  CRLB  is  then  found  by  squaring  both  sides  and  splitting  the  integral  with 


the  Cauchy- Schwarz  inequality: 


(x(C)  -  x)2p(C;  x)  d( ■  J  ^  )  p(C\  x)  d(  >  1  (64) 


The  left  argument  is  recognized  as  the  expected  mean-squared  error  of  the  esti¬ 
mator,  and  the  right  argument  is  defined  as  the  Fisher  Information,  the  variance  of 
the  score  (the  mean  of  the  score  can  be  shown  to  be  zero).  For  an  unbiased  estimator, 
then,  the  CRLB  tells  us  that  the  certainty  with  which  we  know  our  estimate  is  limited 
by  the  Fisher  Information  of  the  likelihood  function: 


Var  [xl  >1  1  (x) 


(65) 


where: 


l(x.)  =  E 


dlnp(Z;x) 

<9x 


(66) 


A  more  useful  formulation  is  found  with  Equation  [60]  and  the  assumption  made 


for  Equation  59  differentiating  the  likelihood  function  with  respect  to  x: 

dp{ C;x)  f°°  <91np(C;x) 


<9x 


p(C;x)dC  (67) 


Assuming  the  second  partial  exists  and  is  integrable,  the  equation  is  differentiated 
again: 


d2  In  p(C; : 
9x2 


'p(C;x)dC  +  /”(Sh^A)  p(C;x)  d(  =  o 


(68) 


which  leads  us  to  the  familiar  form  of  the  FIM: 


X  (x)  =  E 


/ <91np(Z;x)\  2 

=  -E 

d2  In p(Z-,x.) 

V  5x  ) 

dx2 

(69) 
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Taylor  applied  this  form  of  the  FIM  to  a  dynamical  system  with  a  non-linear, 
time- varying  state  vector  under  deterministic  inputs  with  time- varying  measurements 
corrupted  by  additive,  Gaussian  white  noise  sequences  [108].  Because  the  measure¬ 
ments  are  assumed  to  be  independent,  the  likelihood  function  is  a  product  of  the 
individual  Gaussian  exponential  distributions.  Under  the  logarithm,  this  becomes  a 
sum,  allowing  a  recursive  form  of  the  FIM  to  be  found  by  taking  the  expectation  of 
the  second  partials: 


where 
of  the 


'k-\-l\k 


k+ 1 


fc  is  the  state  transition  matrix  from  xtfe  to  xt  ,  and  H 


i-fc+i 


observation  function  from  Equation  11 


(70) 

is  the  Jacobian 


Hfc+i 


<9h  [xj+i] 

<9xfc+1 


l-tan-1^  ftan-1^ 

dx  Xrk  +  1  dz  Xrk+1 


rk+ 1 


1  + 


Zrk  + 1 
Irk+ 1 


rk+ 1 


1  + 


'rk  + 1 
Crk+ 1 


-1 
"rk+ 1 


Zrk  + 1  Xrk+ 1 

Pk+1  Pk+1 


(71) 


Equation  [70]  shows  that  the  amount  of  information  that  each  measurement  provides  is 
encapsulated  in  the  term  Assuming  a  constant  uncertainty  for  each 

measurement,  erg,  the  directional  information  is  contained  within  H^+1Hfe+i.  The 
same  conclusion  is  reached  if  the  problem  is  addressed  with  a  least  squares  approach 
on  a  Taylor  series  expansion  expanded  around  a  nominal  state,  as  is  done  in  the  GPS 
dilution  of  precision  (DOP)  analysis  (H7  H  is  known  as  the  DOP  matrix  [ST]),  flipping 
the  problem  to  use  measurement  uncertainty  in  angle  vice  the  GPS  uncertainty  in 
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range.  Unsurprisingly,  this  also  can  be  seen  in  the  observability  Grammian  as  well  [78]: 


M  =  r  (r,  t0)  HT  (r)  H  (r)  $  (r,  t0)  dr 

J  to 


(72) 


In  the  polar  formulation,  the  transition  matrix  rotates  the  information  matrix  to 
the  new  orientation  of  p  and  f3  as  the  observer  moves  in  relation  to  the  target.  In 
the  Cartesian  formulation,  however,  the  estimate  of  the  target  state  is  anchored  to 
the  navigation  frame,  which  does  not  change  as  the  relative  coordinates  vary.  The 
state  transition  matrix  is  identity.  If  the  certainty  in  the  observer’s  position  has 
reached  a  steady-state,  then  the  only  time  the  information  certainty  in  the  target 
estimate  changes  is  when  there  is  a  measurement  update,  removing  process  noise 


from  consideration.  Using  Equation  70  and  the  appropriate  trigonometric  identities, 
the  Fisher  Information  Matrix  becomes: 


Xi.  —  Xn  H - 7T 


a% 


E 

i=l 

k 


sin2  ft 


E 

i=  1 


sin  ^  cos  Pi 


-E 

i= 1 
k 


sin  Pi  cos  Pi 


E 

i=  1 


cos  2  Pi 

Pi 


(73) 


5.1.1  Directional  Compression  and  One-Step  Ahead  Analysis. 


Many  of  the  trajectory  planning  techniques  currently  in  use  compress  metrics 


similar  to  Equation  [73]  into  a  scalar  to  determine  the  optimal  path.  This  can  be 
instructive.  Applying  a  one-step  ahead  approach  and  using  the  determinant  of  the 
FIM  as  the  metric  of  choice,  the  question  becomes  how  to  maximize  the  information 
in  the  next  step.  Adopting  the  abbreviations  Sf.  =  sin and  C*,  =  cos/3fc,  the 
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determinant  becomes: 


det  (H^Hfc  +  H^+1Hfc+1)  = 


1  c2  i  1  c2 
pI bk  +  Pl+ ik+1 
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sin  (pk+1  +  pk)  sin  (/3fe+i  -  fik) 
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(75) 


This  equation  gives  insight  to  the  geometry  of  the  problem,  and  supports  natural 
intuition.  Subsequent  measurements  from  the  same  angle  yield  no  new  information — 
neither  do  measurements  from  an  opposing  angle  across  the  target  (difference  of  tv). 
To  accomplish  the  goal  of  minimizing  the  area  of  uncertainty  around  the  target  loca¬ 
tion  estimate  (from  any  fixed  pk  and  (3k),  the  observer  should  move  in  such  a  manner 
to  decrease  the  range  and  increase  the  orthogonality  of  the  next  measurement.  Note, 
however,  that  the  information  about  the  shape  of  the  uncertainty  ellipse  has  been 
lost  in  the  compression.  Initial  efforts  for  this  research  treated  this  one-step  ahead 
approach  as  an  optimal  problem,  analytically  solving  for  a  control  policy  analogous 
to  mu.  The  result  was  a  spiral  toward  the  target  very  similar  to  that  of  [SB]. 


Though  implementablc  in  real-time,  this  localization  approach  was  not  optimal 
over  the  entire  trajectory,  and  failed  to  address  a  significant  number  of  the  limitations 
of  previous  research.  Most  glaringly,  the  future  value  for  the  covariance  at  the  end 
of  the  path  is  unknown,  and  the  shape  of  that  uncertainty  ellipsoid  is  lost  in  the 
compression.  Real  systems  require  a  trajectory  that  will  allow  them  to  achieve  a 
particular  certainty  magnitude  and  shape  determined  by  physical  realities.  A  method 
was  sought  to  reach  a  particular  final  certainty,  based  on  the  true  system  requirements. 


5.2  A  New  Approach 

Localization  and  dual  control  methods  compress  an  information  metric  to  a  scalar 
performance  index  and  seek  a  control  that  will  maximize  the  amount  of  information. 
This  may  or  may  not  meet  the  certainty  requirements  of  a  system  based  on  physical 
realities — the  required  fire  control  solution  to  launch  a  torpedo  for  the  submarine, 
the  size  and  shape  of  the  hook  used  for  a  sUAS  to  land  on  a  wire,  etc.  If  a  path 
received  from  a  trajectory  planner  balances  a  weighted  effort  on  localization  and 
control,  the  ending  certainty  level  is  unknown.  If  requirements  are  not  met,  the 
mission  results  in  failure.  If  requirements  are  over-met,  the  solution  may  have  met 
optimality  conditions,  but  the  cost  function  did  not  match  the  true  needs.  In  that 
case,  the  trajectory  planner  produced  the  right  solution  to  the  wrong  problem. 

The  intent  of  this  dissertation  is  to  fundamentally  change  the  way  the  dual  control 
problem  is  approached.  For  systems  where  a  level  of  information  is  a  necessary, 
but  secondary  tool  required  to  perform  a  primary  mission,  effort  and  energy  should 
not  be  wasted  on  information-gathering  maneuvers  that  are  unneeded.  The  path 
planning  algorithm  should  not  seek  to  maximize  certainty,  nor  to  nebulously  balance 
the  amount  of  effort  based  on  the  certainty  you  have  now,  especially  if  the  geometry  of 
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the  problem  is  such  that  the  certainty  level  will  greatly  change  soon.  The  amount  of 
certainty  expected  at  the  final  condition,  the  point  of  mission  accomplishment,  should 
be  what  drives  maneuvers — not  the  current  state  and  estimate.  The  final  expected 
uncertainty  is  a  particular  amount  of  information  in  each  direction,  dependent  on  the 
system  and  the  mission. 

5.2.1  Suboptimal  Final  Covariance  Shooting  Method. 

To  begin  the  process  of  evaluating  a  path  based  on  the  final  estimate  error  uncer¬ 
tainty,  a  shooting  method  was  developed.  This  method  used  ideas  similar  to  some 
of  the  dual  control  methods  that  selected  weights  to  balance  control  and  estimation 
efforts.  Instead  of  basing  the  weights  on  current  certainty  levels,  the  shooting  method 
uses  the  future  certainty  expected  at  the  critical  moment.  As  a  circular  argument, 
an  iteration  was  introduced  to  optimize  on  the  correct  set  of  weights,  analogous  to 
indirect  optimal  shooting  approaches.  The  weights  are  adjusted  until  the  optimal 
path  contains  the  desired  characteristics  of  the  prescribed  final  uncertainty  levels  in 
each  direction,  assuming  that  measurements  will  continue  to  be  received  along  the 
route.  Using  the  orthogonality  lessons  from  Equation  [75]  and  the  polar  formulation, 
the  cost  function  was  proposed: 

rtf 

JsuboPt  =  wttf+  j  — ryxsin2/3  —  wzcos2f3  +  Uy  WUyuy  dt  (76) 

Jto 

The  final  time  requirement  ensured  that  unnecessary  maneuvers  were  not  accom¬ 
plished,  and  the  sine  and  cosine  terms  ensured  that  information  from  both  orthogonal 
directions  was  gained.  A  weakness  of  this  method  (and  many  of  the  dual  methods) 
is  the  failure  to  account  for  the  fact  that  measurements  at  a  close  range  provide  a 
higher  level  of  information.  Initial  weights  were  selected  based  on  simulation  of  the 
path  that  will  be  solved  with  the  initial  guess,  since  that  is  known  a  priori.  As 
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measurements  are  received  and  the  estimate  of  the  target  location  begins  to  move,  an 
inner  iteration  loop  is  accomplished.  First,  the  optimal  path  is  solved  for  based  on  the 
initial  weights.  The  measurement  function  is  then  linearized  about  each  anticipated 
measurement  along  that  path,  and  the  EKF  update  equations  are  used  to  propagate 


the  certainty  for  all  expected  measurements,  as  done  in  Equation  48  on  page 


The  result  provides  the  entire  expected  covariance  matrix  at  the  final  time,  allowing 
decisions  to  be  made  directionally,  vice  only  being  able  to  work  with  a  scalar  approx¬ 
imation.  The  weighting  is  then  adjusted  based  on  the  future  expected  uncertainty, 
and  the  loop  is  continued  until  tolerances  are  met. 

A  heuristic  function  is  required  to  adjust  the  weights.  The  weight  on  the  controls 
is  held  fixed,  and  the  weighting  on  the  directional  information  is  determined  by  a 
ratio,  resulting  in  two  “knobs”  to  adjust  the  path — one  on  direction  ratio,  wx,  and 
one  on  the  final  time,  Wt ■  If  the  final  expected  covariance  in  the  x-direction,  Pxx, 
does  not  meet  the  requirements,  its  weight  is  increased  in  relation  to  that  on  Pzz 
( wz  =  1  —  wx ).  If  the  certainty  in  both  directions  exceeds  the  required  standard, 
the  path  can  be  made  shorter,  and  the  relative  weight  on  the  final  time  is  increased. 
Families  of  solutions  can  be  produced  by  tuning  the  two  “knobs,”  wt  and  wx,  as  shown 
in  Figure  [I7j 

This  method  overcomes  some  of  the  major  limitations  of  previous  dual  control 
approaches.  Besides  being  able  to  provide  the  requirement  of  a  final  uncertainty,  the 
system  no  longer  has  hnal  time  as  a  fixed  entity.  This  is  critical,  as  the  path  may 
need  to  be  shortened  or  lengthened  for  more  measurements  in  response  to  physical 
certainty  requirements.  Two  paths  are  shown  in  Figure  [l8j  In  both  cases,  the  initial 
geometry  of  the  problem  makes  getting  information  in  the  z-axis  direction  easy,  while 
the  x-axis  direction  is  initially  unobservable  and  requires  maneuver  to  achieve  the 
necessary  observability.  The  Erst  profile  represents  a  solution  where  a  low  amount  of 
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Figure  17.  Iterative  Method  of  Shooting  for  Final  Covariance 
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(b)  Profile  2,  wt  =  0.5,  wx  =  0.55, 
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Figure  18.  Flightpaths  with  Different  Levels  of  Required  Final  Covariance 


72 


additional  information  is  required  over  the  current  levels  of  certainty,  and  the  second 


path  is  representative  of  a  path  with  much  greater  need  for  information 
the  speed  profile  differences  in  the  details  of  Figure  19  and  Figure  [20} 
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Figure  19.  Profile  1,  High  Total  Speed  for  Entire  Flight 


case,  a  maximum  speed  profile  is  optimum,  while  in  the  second,  the  optimum  profile 
is  to  move  at  maximum  speed  to  an  angle  nearly  orthogonal  to  the  a:-axis,  and  then 
to  dwell  at  a  very  low  speed — collecting  additional  measurements  to  increase  the 
certainty  in  the  x-direction. 

This  ability  to  change  speed  and  path  length  far  exceeds  the  current  methods  of 
dual  control,  which  rely  on  fixed  numbers  of  measurements  (fixed  final  time)  and  fixed 
velocities  in  the  solution.  The  dual  control  solutions  are  optimal  in  a  mathematical 
sense,  but  unless  you  happen  to  pick  the  optimal  number  of  measurements  for  your 
needs  and  the  optimal  speed,  the  solution  isn’t  really  what  you  are  looking  for.  This 
deficiency  can  clearly  been  seen  in  Figure  [7  on  page  29 
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Figure  20.  Profile  2,  High  Speed  to  Good  Observation  Point,  Followed  by  a  Dwell  to 
Collect  Extra  Measurements 


5.2. 1.1  Shooting  Method  Limitations. 

There  are  several  drawbacks  to  this  shooting  method,  with  two  that  particularly 
stand  out.  The  first  is  the  requirement  for  a  heuristic  program  to  search  for  the 
weighting  combination  that  will  result  in  the  right  final  characteristics.  There  are 
many  potential  local  minimums  in  this  choice,  as  there  are  potentially  any  num¬ 
ber  of  weighting  combinations  that  may  be  sufficient  given  two  “knobs”  to  adjust. 
Mathematically,  a  global  minimum  could  be  attained  by  assuming  a  weight  ratio  to 
prescribe  the  balance  between  time  and  direction  efforts,  thereby  reducing  the  scope 
of  the  problem  to  only  one  tuning  parameter,  but  making  that  assumption  would 
further  limit  the  optimality  of  the  solution. 

The  obvious  second  drawback  is  the  inefficiency  involved  with  having  two  opti¬ 
mization  loops.  Not  only  does  the  system  have  to  iterate  to  find  the  optimal  solution 
for  each  set  of  weights,  but  it  must  iterate  to  find  the  optimal  weights  to  supply  the 
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required  certainty  levels — for  every  planning  epoch.  Each  time  the  system  receives 
a  new  measurement,  the  estimated  target  location  moves,  invalidating  the  previous 
solution  and  the  process  must  begin  again.  In  theory,  these  updates  will  increase  in 
speed  as  the  target  estimate  becomes  more  certain  with  many  measurements,  and 
using  the  previous  solution  as  a  “bootstrap”  guess  will  speed  up  computation  time, 
but  the  process  is  too  inefficient  for  a  real-time  program.  A  smooth,  efficient,  single- 
shot  solution  was  desired — one  that  incorporates  the  shooting  method’s  gains  of  a 
determined  final  covariance  and  a  flexible  number  of  measurements,  but  that  solves 
the  optimal  control  and  the  optimal  estimation  problems  in  a  single  epoch. 

5.2.2  Single-Shot  Simultaneous  Control  and  Estimation. 

In  order  to  overcome  the  limitations  of  all  of  the  localization  and  dual  control 
methods  addressed  in  this  dissertation,  the  basic  approach  to  the  formulation  of  the 
optimal  control  problem  must  be  fundamentally  altered.  Instead  of  optimizing  on  a 
particular  information  metric,  or  balancing  control  and  estimation  desires  (based  on 
that  information  metric),  a  general  cost  function  should  be  allowed  that  encapsulates 
the  control  desires  for  mission  accomplishment  for  any  given  system.  In  the  absence 
of  a  need  for  additional  information,  this  cost  function  should  result  in  a  solution 
that  follows  the  most  desired  path,  be  it  minimum  time,  minimum  energy,  or  any 
other  function.  The  final  error  covariance  requirements  must  be  removed  from  the 
performance  index  and  be  addressed  as  they  really  are — a  constraint.  If  the  path 
requires  more  maneuvering  to  achieve  a  better  final  target  estimate,  the  path  planner 
should  determine  how  much  and  in  what  directions,  deviating  from  the  intent  of  the 
general  cost  function  as  little  as  possible.  If  the  mission  can  be  accomplished  in  the 
optimal  manner  without  additional  information,  the  solution  should  be  found  as  if 
observability  was  not  considered. 
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Though  straightforward  in  theory,  this  concept  is  problematic.  The  final  error 
covariance  cannot  simply  be  applied  as  a  final  state  combination  constraint,  since  the 
problem  is  non-holonomic.  There  is  no  way  to  calculate  the  final  certainty  based  only 
on  the  final  point — the  entire  path  must  be  considered.  One  possible  solution  would 
be  to  solve  the  entire  path  first,  and  then  propagate  the  Kalman  filter  equations 
forward  to  see  if  the  path  met  observability  requirements  (this  is  the  essence  of  the 


shooting  method  in  Section  5.2.1).  This  method,  however,  does  not  provide  the 


optimal  control  solver  with  any  path  gradient  information  for  how  to  change  the 
path  in  order  to  improve  the  characteristics  (hence  the  weight  iteration  scheme  of  the 
shooting  method). 

To  get  the  information  of  how  to  change  the  path  for  observability  requirements 
into  the  context  of  the  optimal  control  solver,  the  uncertainty  information  must  be 
contained  within  the  states,  or  be  contained  within  additional  appended  states.  Only 
in  this  manner  will  the  constraint  Jacobian  contain  the  gradient  information  necessary 
to  correctly  move  the  path.  To  do  this  requires  a  method  that  will  quantify  how  the 
level  of  information  changes  with  respect  to  time,  in  relation  to  a  particular  system 
state  vector. 

Attaining  an  appropriate  dynamical  equation  is  problematic  for  a  continuous  for¬ 
mulation,  as  the  information  changes  are  characterized  by  steps  at  discrete  times  when 
measurements  are  received.  A  fixed  time  step  could  potentially  be  assumed  and  the 
optimal  control  problem  attempted  with  equally  spaced  nodes  in  a  parameterized 
system,  but  sacrificing  the  pseudospectral  node  spacing  of  modern  direct  methods 
means  giving  up  speed  and  accuracy  desired  for  an  on-line  system. 

Maybeck  presents  a  continuous  equation  for  propagation  of  uncertainty  matrices 
within  the  context  of  the  linear  Kalman  filter 


P(t)  =  F(t)P(t)  +  P{t)FT(t)  +  G(t)Q(t)G(t)  -  P(t)W  (f)R“1H(t)P(t)  (77) 
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In  this  equation.  F  contains  the  state  propagation  information.  For  the  polar  for¬ 
mulation,  this  rotates  the  covariance  matrix  to  align  with  the  changing  states  of  p  and 
/3.  For  the  Cartesian  formulation,  which  is  tied  to  the  inertial  frame,  F  =  0.  In  the 
third  term,  G  encapsulates  the  input-output  transfer  functions,  which  regulate  the 
influence  of  the  assumed  dynamics  noise,  described  by  Q.  This  adds  the  increasing 
covariance  trait  between  measurements.  For  this  problem,  since  the  target  is  static 
and  the  own-ship  position  estimate  is  assumed  to  have  achieved  steady-state,  the  error 
covariance  does  not  change  between  measurements,  so  G  =  0  as  well.  It  would  seem 
then,  that  the  dynamics  of  P  could  be  estimated  by  P (t)  =  — P(t)FIr(t)R(71H(t)P(t). 
In  that  case,  the  elements  of  the  covariance  matrix  could  be  appended  to  the  state 
vector,  and  limited  to  the  desired  final  required  covariance  size  and  shape  with  appro¬ 
priate  boundary  conditions.  To  achieve  Equation  [77j  however,  a  simplifying  assump¬ 
tion  of  continuously  available  measurements  was  made.  For  the  sUAS  scenario  using 
line  detection  algorithms  on  sequential  images  for  measurements,  the  expected  update 
rate  was  between  2  and  3  Hz.  Allowing  a  continuous  measurement  assumption,  and 
allowing  the  accompanying  linearization  of  the  system,  the  resulting  covariance  esti¬ 
mate  is  not  responsive  enough,  particularly  to  the  first  measurement,  and  the  error 
is  slow  to  correct,  as  shown  in  Figure  |2lj 

To  incorporate  the  measurement  sample  time  into  an  approximation  for  the  covari¬ 
ance  dynamics — again  assuming  that  the  only  change  happens  at  the  measurement 
update — a  single  update  equation  can  be  used: 

P(W  =  P(«.+) 

=  P(<D  -P(«r)HT(i.)[H(t.)P(i-)HIy.)  +R(i.)]_1H(«i)P((-)  (78) 
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^  100  — Truth 


Figure  21.  Inadequacy  of  Continuous  Measurement  Assumption  for  Covariance  Prop¬ 
agation 
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(79) 


Clearly,  this  first-order  approximation  is  only  accurate  for  small  values  of  A tmeas, 
and  is  questionable  at  best  for  this  application.  Even  if  accurate,  however,  attempting 
to  iterate  within  the  context  of  an  optimal  control  solver  when  determination  of  the 
state  dynamics  at  every  step  of  every  iteration  includes  multiple  matrix  multiplica¬ 
tions  and  an  inverse  can  result  in  poor  performance  and  numeric  instability. 

5.2.3  Information  States  and  Associated  Dynamics. 

The  principles  of  the  FIM  can  be  used  to  address  this  problem.  The  FIM  contains 
all  of  the  required  directional  information  necessary  to  direct  the  optimal  path  plan¬ 
ner,  so  a  method  for  inserting  that  information  into  the  context  of  the  optimal  control 
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problem  was  developed  as  follows.  Equation  [73]  defining  the  FIM  for  this  application 
is  repeated  here  for  convenience: 


I k 
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(80) 


Allowing  the  assumptions  that  measurements  will  continue  to  be  received  every 
Atmeas  seconds,  and  that  the  standard  deviation  of  each  measurement,  erg,  is  constant 
for  all  measurements,  an  integral  may  be  used  to  approximate  the  discrete  steps  of 
the  measurement  updates,  similar  to  the  Euler- Maclaurin  formula: 


%{t)\  t=t,  =  %o  + 


rtk  sin 2f3(t)  j,  _  rtk  sin  f3(t)  cos  f}(t)  >, 

Jto  A  tmeasVpp2(t)  J  to  Atmeas<?'p  p2  ( t ) 

rtk  sill /3(t)  cos  f3(t)  ,,  rtk  cos 2  fi(t)  ,, 

Jto  A tmeas<rpP2(t)  J  t0  AtmeasCT^p2  (t) 


(81) 


Recalling  that  Ik  =  Pfc  1  for  an  efficient  estimator,  continuous  information  states, 
£j(f),  are  defined  based  on  the  elements  of  this  FIM  approximation  such  that: 
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where  [P  1(to)]jj  refers  to  the  ijth  component  of  the  matrix  at  time  to-  Clearly: 


I{t)  —  X0  + 


Itl  t  (t)  dt  Jl  &  (f)  dt 
Jtjsit)  dt  jit  it)  dt 


(83) 
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The  dynamics  of  the  information  states  can  then  be  found  from  the  derivative  of 
the  FIM  approximation: 

dl{t) 
dt 

With  the  dynamics  available,  and  the  initial  conditions  found  from  the  inverse  of 
the  initial  covariance  matrix,  the  information  states  may  be  appended  onto  the  state 
vector  in  the  optimal  control  problem.  The  approximate  FIM  may  then  be  formu¬ 
lated  at  any  point  in  time,  the  inverse  of  which  should  yield  a  close  approximation 
to  the  covariance  at  that  time.  Looking  forward  using  results  of  the  actual  flight 
tests,  Figure  [22]  shows  a  qualitative  example  of  the  accuracy  of  the  approximation 
by  post  processing  flight  test  data  from  Run  #1,  the  first  run  with  an  actual  wire. 
The  approximation  data  are  covariance  elements  calculated  with  the  inverse  of  the 
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Figure  22.  Ability  to  Accurately  Approximate  Covariance  with  Information  States, 
Flight  Test  Run  #1 


approximate  FIM,  which  was  assembled  from  the  information  states.  The  truth  data 
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are  generated  by  applying  the  Extended  Kalman  Filter  measurement  equations  in  the 
Cartesian  formulation  at  the  measurement  update  times: 


Pfc|fc— 1  K-fcHfcPfci/j—i  (85) 


By  constructing  the  FIM  from  the  information  states  and  taking  its  inverse,  this 
method  provides  a  way  to  bring  the  information  contained  in  the  error  covariance 
matrix  into  the  context  of  the  optimal  problem  in  a  manner  that  provides  a  gradient 
for  how  to  change  the  path  to  affect  certainty  directionally.  In  this  manner,  with 


some  considerations  that  are  addressed  in  Section  |5.3[  the  error  uncertainty  at  the 
final  time — the  true  mission  requirement  for  the  bearing-only  systems  addressed  in 
this  dissertation — can  now  by  explicitly  prescribed  through  a  multi-state  boundary 
condition. 


5.3  Optimal  Control  Problem  Formulation 

The  optimal  control  problem  for  each  epoch  of  the  real-time  trajectory  planner 
can  now  be  formulated  using  an  augmented  state  vector: 


x  = 


T 


X  Z  Vx  Vz  &  £3 


(86) 


Control  is  as  defined  in  Equation  17  on  page  46|  with  the  limitations  described 
there.  In  Bolza  form,  the  optimal  control  problem  is  to  determine  the  state-control 
function  pair,  {x  (f)  ,  u  (£)},  and  final  time,  tf  (in  this  case  to  is  known  for  each  epoch), 
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which  minimize  the  cost  functional: 


J  =  r  (x(t0)  ,tQ,Z(tf)  ,tf)  +  [  £(x(t),u(i),i)  dt  (87) 

Jt0 

subject  to  the  dynamic  constraints: 

dx/dt  —  f  (x  (t) ,  u  (t) ,  t )  (88) 

the  path  constraints: 

C  (x(£)  ,  u(t)  ,t0,tf)  >  0  (89) 

and  the  boundary  conditions: 

7(x(*o)  >t0,x{tf)  ,tf)  >  0  (90) 

with  equality  constraints  imposed  via  a  second  constraint  on  the  additive  inverse. 

The  advantage  to  this  new  method  of  incorporating  final  covariance  as  an  event 
constraint  (a  multi-state  boundary  condition)  in  the  optimal  control  problem  is  that 
a  general  performance  index  can  be  used  to  best  fit  the  situation.  Note  that  the  final 
time  should  remain  free.  Previous  methods  have  defined  a  fixed-final-time  horizon, 
or  have  implicitly  done  so  by  fixing  the  number  of  measurements.  A  free  final  time 
allows  alteration  of  the  number  of  measurements  received,  which  can  greatly  impact 
the  solution.  The  vehicle  must  have  the  ability  to  slow  down  in  an  area  (or  lengthen 
the  portion  of  the  path  that  is  in  a  certain  direction  for  fixed  velocity  problems)  if 
more  measurements  are  required  from  that  aspect  angle. 

For  the  sUAS  landing-on- a- wire  scenario,  the  final  time  was  selected  to  be  mini¬ 
mized  with: 

r  =  t,  (9i) 
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The  states  are  free  to  move  without  penalty  within  the  limitations  of  C,  but 
weighting  could  easily  be  added  in  other  applications  for  best  possible  tracking  or 
avoidance  of  areas  while  still  gaining  the  required  certainty  for  mission  accomplish¬ 
ment.  A  small  penalty  was  added  on  control: 

C{t)  =  uT(f)W„u(t)  (92) 


with  the  weights  in  W„.  set  to  0.1  on  each  diagonal  element.  The  addition  of  a  small 
weight  on  control  is  an  effective  method  of  avoiding  numerical  instabilities  associated 
with  optimal  problems  posed  on  a  singular  arc. 

5.3.1  Avoidance  of  the  Singular  Arc. 

A  brief  analytical  look  at  the  problem  sheds  light  on  the  singular  arc  issue,  a 
recurring  issue  for  many  numerical  problems.  Constraints  will  be  detailed  in  the  next 
section,  but  for  now,  none  of  the  constraints  in  this  particular  formulation  include  a 
combination  of  states  and  controls,  and  they  are  not  functions  of  the  initial  or  final 
time,  allowing  them  to  be  split  into  constraints  on  the  state  vector  and  constraints 
on  the  controls,  respectively: 

C(x(t),u(t),t0,*/)  =  {Cx(i(t)),Cu(u(t))}  (93) 


Defining  Lagrange  multipliers,  A  ft),  and  the  unit  Heaviside  step  function: 


{0,  for  C i  (x  (t))  >  0 

1,  for  Cj  (x  (t))  <  0 


(94) 
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The  Hamiltonian,  T~L,  can  then  be  defined  using  the  variational  approach  for  prob¬ 
lems  with  state  variable  inequality  constraints  in  [66J  by  defining  a  new  state  variable: 


is(t)=  [Cf(S(i))]2H(-Gf)+  [C2s(S(i))]2H(-Cf)  +  .-.+  [C*,(X(«))]2H(-G*S) 

(95) 

for  the  nCx  constraints  in  Cx.  The  derivative  of  x%(t)  is  always  positive,  and  the 
value  for  the  state  is  kept  at  zero  by  enforcing  boundary  conditions  of  £8(f0)  =  0  and 
x$ (tf)  =  0,  thereby  enforcing  the  state  inequality  constraints  for  all  time. 

The  Hamiltonian  for  the  now  n  +  1  states  can  then  be  expressed  as: 

n(Z(t)Mt),  X{t),t)  =c(x  (t) ,  U (t),  t)  +  Ai(f)/i  (x(i),  u (t),t) 

H - f  A n(t)fn  (Z(t),u(t),t) 

+  A n+1(t)  [Cf  (x(t))]2H(-Cf)  +  •  •  •  +  [C^  (x  (t))]2U(-Cl J 
=  C(x{t),u  (■ t ) ,  t)  +  X  T(t)f  (x  (t) ,  u  (t) ,  t)  (96) 


In  a  case  where  control  was  unconstrained,  the  sufficient  optimality  condition  for 
control  would  yield: 

dn(Sc*(t),u*(t),X*(t),t)  =  o 

dn 

As  the  controls  are  constrained  for  this  problem  by  Cu,  which  defines  the  admissi¬ 
ble  controls  u  G  U,  Pontryagin’s  maximum  principle  (or  minimum  in  this  case)  must 
be  applied: 

u*  =  arg  111111?/  (98) 

u*eu 

Without  the  addition  of  the  quadratic  term  that  was  added  in  C,  none  of  the 
control  terms  in  the  Hamiltonian  are  higher  than  first-order,  meaning  that: 


d2H 

<9u2 


0 


(99) 
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Because  <92"H  / du2  is  singular,  u  is  not  uniquely  defined  by  the  optimality  condition — 
this  is  the  definition  of  a  singular  arc  [7J.  There  are  several  methods  of  dealing  with 
singular  arcs,  but  most  of  them  involve  substantial  insight  into  the  shape  of  the  op¬ 
timal  solution,  taking  time  derivatives  of  dT-i/dn  until  the  control  does  show  up,  or 
reformulating  the  problem  into  one  without  a  singular  arc.  Many  numeric  methods 
rely  on  the  Hessian  for  direction  and  step  size  information.  Adding  a  very  light  control 
cost,  as  in  Equation  [92j  can  eliminate  much  of  the  volatility  that  can  be  associated 
with  numeric  optimal  solutions  on  a  singular  arc.  If  there  is  no  noticeable  change  to 
the  optimal  trajectory,  or  if  the  changes  are  acceptable  for  the  system  in  question, 
this  technique  provides  a  simple  method  for  smoothing  the  control  solution  provided 
by  numeric  solvers. 


5.3.2  Constraints. 


The  Dynamic  constraints,  /,  of  Equation 


were 


defined  by  Equations  11 


and  84  Path  constraints  were  applied  to  scale  the  problem  within  the  physical 
limitations  of  the  available  indoor  flight  test  facility  in  order  to  make  use  of  the 


Vicon  motion  capture  system,  described  in  Chapter  Vlll  In  practice,  the  optimization 
software  used  required  inequality  constraints  on  all  states  and  controls.  Variables  not 
intended  to  be  constrained  had  constraint  values  set  well  out  of  a  realistic  range,  but 
not  at  infinity  to  keep  gradients  meaningful.  The  potentially  active  constraints  of  C 
are  shown  in  a  consolidated  notation: 


—9  m 

X 

Tipp  offset.  T  Xf 

0.8  m 

z 

5.5  m 

—0.5  m  / s 

< 

VX,VZ 

< 

0.5  m  j s 

—0.5  m  / s2 

ux,  uz 

0.5  m  / s2 

1 

O 

O 

CO 

1 

p 

o 

o 
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The  forward  horizontal  component,  x,  was  limited  to  stay  within  the  boundaries  of 
the  indoor  flight  test  facility  and  the  expected  approach  point  defined  relative  to  the 
wire  position  estimate,  rrapP_offset  +  xt.  The  approach  point  itself  is  only  an  estimate, 
changing  each  epoch,  but  it  does  provide  some  safety  buffer  until  the  desired  target 
certainty  is  reached.  Vertical  limits  were  set  so  that  the  landing  gear  would  clear  the 
floor,  and  the  “ceiling”  limit  ensured  that  the  vehicle  would  stay  low  enough  to  remain 
visible  by  a  sufficient  number  of  Vicon  cameras.  For  the  true  sUAS  scenario,  the  upper 
“ceiling”  limit  could  be  removed  in  the  absence  of  airspace  limitations,  simplifying 
the  problem  for  the  optimal  solver.  The  vertical  “ground”  limit  could  be  replaced 
with  a  terrain  model  or  a  min-safe  altitude  for  terrain  avoidance,  as  appropriate. 

The  vehicle  speed  was  also  limited,  increasing  the  total  engagement  time  to  make 
it  representative  of  an  actual  approach.  This  allowed  for  a  realistic  test  of  the  ability 
of  the  RTOC  system  to  control  in  real-time  despite  the  inherent  computational  delays. 
The  path  constraint  on  /3  was  intended  to  keep  the  vehicle  in  a  position  for  the  fixed 
camera  to  maintain  the  wire  within  the  FOV.  The  hat  notation  is  kept  to  denote  that 
the  value  is  calculated  using  the  current  target  estimate,  as  the  true  FOV  limits  are  not 
known.  No  measurements  are  received  when  outside  the  true  FOV.  For  the  quadrotor, 
this  is  always  the  case  as  the  vehicle  transitions  to  land-mode  and  flies  underneath 
the  wire,  but  could  potentially  happen  during  the  flight  due  to  disturbances  or  a  bad 
target  estimate.  If  possible  for  a  full  system,  it  is  recommended  that  the  camera  and 
hooking  method  be  designed  to  keep  the  wire  within  the  sensor  FOV  until  connected, 
to  allow  the  ability  to  correct  for  swinging  wires,  wind  gusts,  and  other  endgame 


disturbances. 


5.3.3  Boundary  Conditions  and  Formulation  of  Final  Covariance 


Constraints. 


The  solution  of  the  optimal  control  problem  is  iterative.  Initial  conditions  for 
each  epoch  are  not  the  current  conditions,  but  the  expected  conditions  at  the  time 
the  next  solution  is  expected  to  become  available.  Based  on  experience  with  current 
hardware,  a  complete  loop  time,  A tcaic  =  0.9  seconds,  is  assumed  inclusively  for  the 
optimization  problem,  the  estimation  problem,  and  all  transport  delays.  The  very 
first  solution  is  seeded  with  zeros.  After  one  solution  exists,  position  and  velocity 
initial  conditions  are  taken  from  the  time  history  of  the  previous  epoch’s  solution, 
xy.(t),  propagated  forward  by  A tcaic\ 


x0 


k+ 1 


Vo 


fc+1 


l  T 


Xfc  (t  T  At  calc)  Vfc(f  T  At  calc) 


(101) 


Care  must  be  exercised  when  initializing  the  information  states,  as  they  are  only 
estimates  of  the  true  FIM  components,  based  on  the  assumption  that  measurements 
will  be  consistently  received  with  a  fixed  time  interval.  The  realities  of  processing 
delays,  poor  image  backgrounds,  and  hardware  issues  in  general  may  lead  to  slow  or 
skipped  measurements.  This  information  must  be  incorporated,  or  the  accuracy  of 
the  information  state  estimates  will  drift  over  time.  As  a  result,  the  initial  condi¬ 
tions  for  the  information  states  are  reset  each  epoch  based  on  the  actual  covariance 
from  the  estimation  filter,  P ,  propagated  forward  by  A tcaic.  To  do  so,  an  expected 
measurement  time  vector  is  created  based  on  the  actual  reception  time  of  the  last 
measurement,  tiast_meas: 


tmeas  \tlast_meas  T  A tmeas,  tlasf_meas  T  2Atmeas, 


?  °tast_meas 


“  1 1  LC.LLO  I 
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where  n  represents  the  maximum  number  of  measurements  that  can  be  incorporated 
such  that: 


tlast_ 


nlS.tr 


^  *o*+i 


(103) 


The  expected  relative  states  at  each  of  these  times  for  epoch  k  are  then  found, 


xr(tmeasJ,  i  —  1 . . .  n.  At  each  point,  the  Jacobian  is  produced  with  Equation  71 


and  the  EKF  update  is  recursively  performed  with  Equation  [85]  The  result  is 
P0fe+i  =  Xkl  ] ,  and  the  elements  of  the  inverse  are  used  as  the  initial  conditions 
for  each  of  the  information  states. 

For  the  terminal  conditions  in  Equation  [90]  the  Erst  four  constraints  of  7  take  the 
system  to  a  hover  at  the  approach  point: 


*£fc+ 1  (tf) 

3^app  _offset  %k 

Zk+l(tf ) 

■^app  .offset  “1“  %k 

vxk+1 (tf) 

0 

_  vzk+1(tf )  _ 

0 

(104) 


For  the  information  states,  the  physical  considerations  of  hook  size,  in  addition  to 
the  steady-state  uncertainty  of  the  vehicle’s  own-ship  position  estimate  determine  the 
hnal  certainty  needs.  For  this  particular  hook  design,  uncertainty  was  best  described 
by  setting  PXXmax  and  P ZZmoI  at  the  hnal  time,  but  any  shape  covariance  ellipsoid 
could  be  specified  based  on  system  requirements. 

To  apply  the  terminal  covariance  conditions  in  terms  of  the  information  states, 
the  elements  were  found  with  inverse  relationships: 


P*»(i/)  =  &((/)  /  [6(f/)&(*/)  -  &’(*/)]  <  P..™.  (105) 

P«(*/)  =&(«/)/  [&(</)&(*/)  -?!(*/)]  <P»™,  (106) 


Care  must  be  taken  in  the  application  of  these  boundary  conditions,  as  the  de¬ 
nominator  can  be  near  singular.  This  makes  taking  the  gradients  of  the  constraint 
problematic  for  the  numerical  solution  and  can  lead  to  instability.  To  avoid  this,  the 
constraint  can  be  re-written  by  noting  that  the  denominator  is  positive.  Proof :  For 
tf ,  Atmeas ,  a /3  G  M1(0,cx)),  incorporating  the  assumption  that  the  vehicle  has  not 
hit  the  target,  xr,zr  G  M1  (— oo,  oo)  :  \xr\  +  \zr\  6  0,  and  assuming  the  system  is 
initialized  with  some  estimate  of  initial  information  with  no  initial  cross-correlation, 
6,6  e  M1  [0,  oo)  :  6(0)  =  6o,  6(0)  =  60,  and  6  e  M1  (-00,00)  :  6(0)  =  0,  then 
for  the  finite  time  span  hi  =  [0,6],  the  information  states  are  defined  everywhere  on 
fk 


.  .  .  f  sin 2f3(t) 

6(f)  =  60+  /  — 7777^  dt  <  00 
Jn  P2(6 

6W  =  6.  +  [  ^  *  <  -  (107) 

Jil  P  (6 

=>-  6,6  e  ^2 


therefore, the  Cauchy- Schwarz  inequality  may  be  used  to  show: 

<■<«•«•  d,  *  L  (=3?)'  • *  *  (/,  =7(7®  •*)’ 

(108) 

=8  6W6W-SW  >0 

A  singular  denominator  would  mean  an  inhnite  uncertainty,  a  condition  that  can¬ 
not  be  returned  to  after  the  finite  initialization,  implying  a  strict  inequality: 


6(66(6  -62 (6  >0  vtGfi 


□ 


(109) 


With  a  positive  denominator,  the  constraints  may  be  rewritten  in  7  to  avoid  any 


numeric  instability  as: 


p**™  [6  (tf)Utf)  -  -  Utf)  >  0 

p**™  [UtfMtf)  -  -  Zi(tf)  >  0 


(110) 


90 


VI.  RTOC  Structure — Requirement  for  Integrated  Error 

Feedback 


“In  theory,  there  is  no  difference  between  theory  and  practice. .  .  In  prac¬ 
tice,  there  is.” 


/■  I  his  chapter  develops  the  feedback  structure  that  should  be  used  in  real-time 
optimal  controllers,  particularly  focusing  on  the  area  of  adding  error  integra¬ 
tion  into  the  recursive  formulation — a  technique  that  has  been  declared  unnecessary 
in  much  of  the  current  research  in  this  relatively  new  held.  The  shortcomings  of  this 
approach  are  shown  through  two  case  studies.  The  first  case  study  is  made  simple 
enough  to  allow  an  analytical  expression  of  the  error  caused  by  choosing  to  use  only  a 
fast  open-loop  recursive  structure,  the  common  approach  in  recent  studies.  Two  more 
appropriate  RTOC  structures  are  suggested,  and  the  second  case  study  implements 
one  of  them  in  a  scenario  likely  to  benefit  from  RTOC — aircraft  attack  planning  in 
the  context  of  pop-up  surface  threats  and  stochastic  disturbances. 


6.1  Fast  Recursive  Open-Loop  Control  vs.  Closed-Loop  Feedback 

The  concept  of  RTOC  is  simple.  Optimal  solutions  are  desired  for  control,  but 
the  solutions  are  only  optimal  for  deterministic  problems.  If  any  of  the  assumed 
parameters  in  the  problem  are  inaccurate  (target  position,  wind,  etc.),  the  solution 
provided  is  most  likely  not  optimal,  and  may  no  longer  be  valid  for  mission  accom¬ 
plishment.  If  a  new  solution  could  be  provided  fast  enough,  however,  the  optimal 
trajectory  could  be  updated  recursively  with  the  most  current  parameter  estimates. 
This  method  typically  includes  “bootstrapping”  the  previous  optimal  solution  as  the 
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guess  for  the  next  epoch,  significantly  decreasing  computation  time.  The  idea  of  a 
recursive  open-loop  solution  is  compelling — once  perturbed  from  the  initial  optimal 
path,  why  waste  control  effort  with  a  feedback  loop  working  back  toward  the  original 
reference  trajectory?  Why  not  find  the  most  optimal  control  now ,  and  apply  that? 
Figure  [23]  illustrates  the  situation. 


Figure  23.  Decision  to  Follow  Initial  Optimal  Trajectory,  or  to  Re-solve  the  Optimal 
Path  from  the  Current  Condition 


Once  the  state  is  perturbed  from  the  expected  optimal  path,  correcting  back  to 
that  trajectory  is  likely  not  optimal  from  the  disturbed  position,  and  a  new  path 
originating  from  the  current  state  should  be  introduced.  Re-solving  the  optimal 
control  problem  as  often  as  possible,  and  maintaining  that  reference  path  between 
optimal  path  updates  with  a  faster,  inner  control  loop  results  in  a  two  degree-of- 


freedom  design,  such  as  the  one  shown  in  Figure  24  which  can  be  found  in  similar 
forms  in  [80j  and  [106J. 


Optimal 


Figure  24.  Two  Degree-of-Freedom  Control  Scheme 
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Recently,  several  authors  have  taken  the  speed  advantages  of  efficient  optimization 
techniques  and  increased  processing  power  to  move  the  control  concept  one  step 
further — eliminating  the  inner  loop  altogether  and  controlling  in  a  purely  recursive 
open-loop  manner.  Conceptually,  if  you  have  control  at  any  point  that  you  have 
defined  as  optimal,  why  would  you  add  anything  to  it?  ft  is  tempting  to  draw  the 
conclusion  that  if  the  recursive  open-loop  optimal  control  can  just  be  solved  fast 
enough,  there  is  no  need  for  feedback,  or  that  recursive  open-loop  control  can  be 
equated  to  feedback  control.  This  proposition  is  a  current  trend  in  the  literature 
for  RTOC  structure  design.  Consider  the  comparison  of  open-loop  recursion  with 
closed-loop  control  in  some  of  those  pushing  the  state  of  the  art  in  the  held  of  RTOC: 


“The  feedback  law  is  not  analytically  explicit;  rather,  closed-loop  con¬ 
trol  is  obtained  by  a  rapid  re-computation  of  the  open-loop  time-optimal 
control  at  each  update  instant.”  [55] 


In  simulated  satellite  guidance,  again  suggesting  that  rapid  open-loop  control 
would  provide  optimal  disturbance  rejection  of  closed-loop  feedback: 


“A  conceptually  simple  approach  to  controlling  such  non-linear  sys¬ 
tems  is  by  solving  the  problems  online.  If  such  problems  can  be  solved 
online,  there  is  no  need  for  an  off-line  design  of  closed-form  feedback 
laws  as,  by  definition,  the  control  system  would  have  acquired  this  intelli¬ 
gence.... Rather  than  tracking  a  pre-computed  solution,  the  control  scheme 
proposed  in  this  paper  re-solves  the  optimal  control  problem  and  updates 
the  control  command  as  soon  as  a  new  solution  is  obtained.  This  results 
in  a  sampled-data  feedback  law  which  provides  optimality  in  the  presence 
of  various  types  of  disturbances.  ”  D3H 


For  simulated  re-entry  vehicle  control: 
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“The  key  for  successful  implementation  of  these  feedback  principles 
relies  on  a  sufficiently  fast  generation  of  open-loop  controls.  Thus,  if  open- 
loop  controls  can  be  generated  as  demanded  by  [a  given  speed  requirement 
for  his  problem],  closed-loop  is  achieved  quite  simply.”  (T0J 


In  a  foundational  work  on  RTOC: 


“Suppose  optimal  open-loop  controls  could  be  computed  in  real  time. 
This  implies  optimal  feedback  control.”  p55| 


and  elsewhere: 


“It  has  been  known  since  the  birth  of  optimal  control  that  if  open- 
loop  controls  can  be  generated  in  real-time,  they  are  basically  equivalent 
to  feedback  controls.  ”  [Ton 

The  concept  that  fast  open-loop  solutions  equate  to  closed-loop  feedback  controls, 
with  the  elimination  of  the  inner  loop  of  Figure  |24|  has  become  pervasive.  While  there 
certainly  is  a  level  of  feedback  that  is  implicitly  achieved  with  a  recursive  open-loop 
structure,  it  falls  far  short  of  “optimal  feedback  control”  in  an  environment  with  any 
true  stochastic  inputs,  as  will  be  shown  below. 


6.2  Lack  of  Error  Integration  in  Instantaneous  Optimal  Solutions 

The  purely  recursive  open-loop  structure  has  shown  success  in  simulations  for 
the  above  problems,  but  lessons  from  classical  control  theory  suggest  significant  lim¬ 
itations  of  this  approach.  The  single  degree-of-freedom  design — removing  the  inner 
feedback  loop — recursively  provides  an  instantaneous  optimal  solution  (future  time 
history)  for  the  control  and  state  (the  faster,  the  better,  in  theory).  While  valuable,  if 
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the  designer  of  a  RTOC  system  makes  the  assumption  that  rapid  open-loop  solutions 
yield  the  same  performance  as  traditional  feedback  control,  the  resulting  design  will 
fail  to  leverage  the  information  that  can  be  gleaned  from  comparing  the  historical 
efforts  to  outcomes. 

The  whole  question  of  RTOC  implies  that  there  are  disturbances  or  unmodeled 
effects  to  be  rejected,  else  the  optimal  solution  would  only  need  to  be  found  once, 
rather  than  in  real-time.  Recursively  solving  the  problem  gives  freedom  to  respond 
to  stochastic  or  unanticipated  effects.  Especially  for  cases  where  these  disturbances 
end  up  not  falling  into  the  classic  categories  of  Gaussian,  white,  and  zero-mean, 
integration  of  the  error  between  the  expected  and  actual  state  and  control  history 
can  supply  either  additional  compensation,  or  a  more  accurate  model  of  the  true 
system  dynamics  through  estimation  of  the  disturbance.  If  the  likely  errors  for  the 
system  are  indeed  non-zero  mean,  or  at  least  time  correlated  (and  thus  likely  non¬ 
zero  mean  over  some  time  interval),  these  effects  should  be  accounted  for  in  selection 
of  the  control.  This  requires  one  of  many  methods  of  feedback  control  that  are  not 
achieved  with  a  purely  recursive  open-loop  design.  Two  non-linear  optimal  control 
problems  are  posed  to  demonstrate  this  principle.  The  first  is  an  overly-simplified 
course  guidance  problem  to  allow  analytic  proof  of  the  error.  The  second  case  study 
will  address  corrective  implementation  in  a  realistic  scenario. 


6.3  Case  Study  A:  Simplified  Aircraft  Course  Planning 


Consider  an  aircraft  simply  modeled  as  a  point  mass  system  with  rectilinear  po¬ 
sition  components: 


*ac(t) 


Vac(t) 


(m) 
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The  aircraft  is  flying  at  a  constant  altitude,  with  a  constant  velocity,  Vac.  The  pilot 
has  been  cleared  direct  to  a  waypoint,  or  fix,  (xaCf,  Uacf ),  and  is  using  the  autopilot 
to  provide  course  guidance.  The  system  dynamics  are  simply: 


Xac(t) 


Vac  COS  i>(t) 
Vac  sin  ip(t) 


(H2) 


where  aircraft  heading,  -0(f),  is  the  control  variable.  Turn  dynamics  are  ignored  for 
simplicity. 

The  optimal  control  problem  is  a  two-point  boundary  value  problem,  with  a  min¬ 
imum  time  performance  index  presented  in  Mayer  formulation: 


Jac  —  tf 


(113) 


Assigning  A (t)  G  l2  as  a  vector  of  Lagrange  multipliers,  the  Hamiltonian  is  defined 


as: 


=  Xi(t)Vac  cos  +  X2(t)Vacsmip(t) 


(114) 


The  first-order  necessary  conditions  provide  the  costate  equations: 


<m 

dxac 

<m 

dyac 


=  AJ(t)  =  0 
=  A  *2(t)  =  0 


(115) 


The  optimality  condition  for  the  unconstrained  control  provides: 


dH 

—  =  0  =  —\*i{t)Vac  sin-?/d(f)  +  X*2(t)Vac  cos i/)* (t) 
dip 


(116) 


a  m 
x  m 


=  tan  ip*(t) 
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(117) 


The  optimal  control  is  therefore  constant,  implying  for  this  case  that  the  state 
dynamics  are  constant,  which  allows  a  solution  for  the  optimal  control  through  simple 
integration  of  both  states  from  the  initial  state  conditions  xac(0)  =  [xaco  yaCQ]T'- 


acf 


=  xaco  +  /  ±*ac(t)  dt 


xaco  +  if 


Vac  COS  Ip* 
Vac  sin  ip* 


(118) 


The  unknown  final  time  is  removed  by  solving  both  equations  for  tf  and  equating 
them,  leaving  the  optimal  control: 


ip*(t)  =  tan 


_  i  /  Vacf  Vaco 
•^acf  ^acg 


(119) 


Note  that  for  recursive  open-loop  control,  the  initial  values  in  Equation  119 


are 


simply  the  current  position  for  each  iteration,  and  the  optimal  control  solved  for  by 
any  method  will  simply  be  a  function  of  the  relative  position  ratio.  Absent  distur¬ 
bances,  the  optimal  path,  and  the  actual  path,  will  unsurprisingly  be  direct  to  the 
target  as  shown  in  Figure  [25| 


6.3.1  Addition  of  Stochastic  Disturbances. 

As  they  are  unknown  beforehand,  the  addition  of  the  typical  zero-mean,  white, 
Gaussian,  stochastic  elements  in  the  forms  of  model  deficiencies  or  disturbances  does 
not  change  the  predicted  solution  for  the  optimal  control.  The  effects  of  disturbances 
can  be  countered,  somewhat,  by  re-solving  for  a  new  optimal  path  at  various  time 


steps,  as  was  illustrated  in  Figure  23  However,  the  production  of  a  new,  instan¬ 


taneous  solution  does  not  provide  anticipation  of  future  disturbance  effects,  or  any 
correction  for  past  errors  or  modeling  discrepancies.  For  unmodeled  effects  which  are 
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A.  Target 

o  Propagated  Location 
— >•  Heading _ 


0  0.2  0.4  0.6  0.8  1 

x  (unit  length) 

Figure  25.  Recursive  Optimal  Control  Solution  with  No  Disturbances,  At=0.1  Units 


more  time  correlated — or  those  that  can  be  characterized  by  an  unknown,  non- zero 
mean — effective  control  requires  some  level  of  feedback,  such  as  integration  of  the 
error  between  the  expected  and  actual  state  path  for  each  step,  or  estimation  of  the 
unknown  parameter (s)  causing  the  disturbance. 

To  illustrate  this,  a  constant  bias,  w,  is  added  to  the  system  in  one  axis.  This  bias 
represents  some  of  the  effects  of  a  wind  component  parallel  to  that  unit  direction. 
Smaller  stochastic  effects  of  the  wind  are  not  modeled  for  this  case  study  in  order 
to  more  clearly  show  the  predominant  impact  and  to  provide  the  opportunity  for 
an  analytical  solution.  The  effects  of  a  time-correlated  noise  source  can  be  seen 
by  simply  replacing  the  experiment  with  a  correlated  function,  w(t).  Even  a  time- 
correlated  function  that  is  zero-mean  overall  can  be  cut  into  segments  of  time  where 
the  mean  is  biased  in  one  direction  or  the  other,  so  the  general  effects  of  the  noise 
contribution  will  be  the  same  as  demonstrated  here,  on  smaller  time  scales. 


The  dynamics  of  Equation  112  become: 


xac(f)  = 


Vac  cos  ip  (t) 

Vac  sin  ip (t)  +  w 


(120) 
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and  the  Hamiltonian  is  updated  to  be: 


'H(xac(t),ij}{t),\(t),t)  =  Ai(i)Kccos^(i)  +  A 2(i)  (V^sin^t)  +  w)  (121) 


The  costate  equations  do  not  change,  and  the  Lagrange  multipliers  are  still  found 
to  be  constant.  The  optimality  condition  shows  that  the  optimal  control  is  constant 
as  well,  allowing  integration  of  the  states  and  removal  of  the  unknown  final  time, 
leaving  the  relationship  for  the  true  optimal  control,  i\)*t : 


Vacf  -  Vaco  _  Vac  Sm  +  W 
Xacf  -  XaCQ  VaC  COS  ^ 


(122) 


The  true  optimal  path  is  shown  in  Figure  26 


with  an  arbitrary  constant  wind 


bias  of  -4  (unit  length) /(unit  time). 


Figure  26.  True  Optimal  Solution,  with  Non-Zero-Mean  Disturbance,  A£=0.1  Units 


If,  however,  the  optimal  steering  is  calculated  without  knowledge  of  the  bias  for 
each  step  of  the  digital  controller,  there  obviously  is  error  between  the  calculated  op¬ 
timal  steering,  i[}*,  and  the  true  optimal  steering,  With  the  appropriate  trigono¬ 
metric  identities,  the  instantaneous  steering  error  from  any  point  may  be  found  by 
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re-solving  the  optimal  problem  from  the  new  initial  location  and  defining: 

4>*e  =  ?/>*  -  i/j*  =  sin-1 


w 


Vac\  /  1  + 


Vacf  Vaco 


%acf  %aco 


(123) 


This  steering  error  results  in  a  “homing”  trajectory  instead  of  a  direct  flight  path,  as 
shown  in  Figure [27fi.  The  key  point  to  emphasize  is  that  this  steering  error  will  always 
exist  (excepting  a  displacement  in  the  direction  of  a  pure  head  or  tail  wind).  Note  that 


Equation  123  is  not  dependent  on  sample  time,  or  the  speed  of  the  recursive  solution 


update,  but  only  on  the  geometry  of  the  problem  at  the  time  of  the  update  and  the 
intensity  of  the  wind.  A  recursive  open-loop  solution  will  always  produce  a  flawed 
steering  solution,  without  the  use  of  some  sort  of  feedback  to  allow  accounting  for  the 
wind  bias.  Attempts  to  increase  the  recursion  rate  may  decrease  the  total  path  error, 


but  never  overcome  the  bias  (analytically  proven  for  this  problem  in  Equation  123) 
Figure  [27|j  shows  a  recursion  rate  of  0.01  time  units. 


0.2  0.4  0.6  0.8 

x  (unit  length) 


1.2  1.4 


Figure  27.  Recursive  Optimal  Solution  with  Non-Zero-Mean  Disturbance  (Homing) 
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These  results  highlight  the  main  lesson  of  this  chapter — the  pitfall  of  assuming 
that  a  high  recursion  rate  on  an  open-loop  optimal  solution  is  equivalent  to  optimal 
feedback  control.  In  the  face  of  non-zero  mean  disturbance,  the  resultant  path  in 
Figure  [27]  is  clearly  short  of  what  would  be  considered  “optimal.”  A  simple  feedback 
scheme  demonstrates  that  the  control  solved  for  through  rapid  recursive  open-loop 


planning  requires  additional  input.  Figures  [28]  and  [29]  show  the  effects  of  adding 
proportional-integral  (PI)  control  in  the  form: 


4>fb(t)  =  kpep(t)  +  hi  /  ep(£)  d£ 

Jt0 


(124) 


where  ep(t)  represents  the  orthogonal  component  between  the  current  position  and 
the  intended  direct  path.  The  gains  were  arbitrarily  selected  as  kp  =  —20  (unit 
length) /radian  and  /q  =  —60  (unit  length) /radian.  The  command,  i/jc,  then  becomes: 


$c{t)  =  ip*(t)  +^fb(t) 


(125) 
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Figure  28.  Optimal  Recursion  with  the  Addition  of  PI  Feedback,  At=0.01  Units 
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Figure  29.  Control  Requirements  with  and  without  Feedback 


The  addition  of  feedback  to  the  recursive  optimal  solution  causes  the  resultant 
path  and  control  to  more  clearly  follow  the  true  optimal  solution,  regardless  of  the 
recursive  update  rate.  In  terms  of  total  time-to-target  (the  objective),  the  analytical 
solution  for  this  particular  example  took  1.092  time  units  to  complete  the  route,  very 
near  to  the  1.094  units  for  the  recursive  open-loop  system  with  feedback,  as  compared 
to  the  1.19  units  with  recursive  open-loop  updates  only. 

Beyond  just  the  timing  differences  and  the  associated  increase  in  fuel  require¬ 
ments,  the  arced  path  of  the  route  found  without  an  inner  control  loop  has  real-world 
navigation  implications.  On  a  regular  basis  under  both  instrument  and  visual  flight 
rules,  aircraft  are  assigned  to  “proceed  direct”  to  a  certain  fix,  or  are  hied  to  proceed 
along  a  route  corridor  by  means  of  navigation  aids  (e.g.  TACAN,  VOR,  etc.)  which 
provide  a  bearing  angle  to  a  fix.  In  either  case,  separation  from  other  aircraft,  clear¬ 
ance  of  terrain,  and  line-of-sight  for  reception  of  the  navigation  aid  signals  is  only 
protected  for  a  narrow  corridor  width.  The  clearance  to  “proceed  direct”  implies  cor¬ 
recting  against  the  winds  to  fly  a  direct  ground  path,  not  merely  homing  to  the  target 
as  you  would  with  a  recursive  open-loop  controller,  which  would  result  in  the  large 
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lateral  excursions  illustrated  in  Figure  [27|  Could  the  RTOC  approach  be  changed  to 
minimize  error  from  a  direct  path?  Certainly,  but  again,  this  implies  implementing 
some  sort  of  explicit  error  feedback.  The  intent  of  this  demonstration  was  to  provide 
a  counter-example  to  the  concept  that  speeding  up  the  open-loop  recursion  rate  was 
equivalent  to  achieving  optimal  feedback  control. 

Therefore,  in  the  design  of  control  schemes  to  implement  RTOC  with  a  fast  open- 
loop  structure,  consideration  of  the  expected  character  of  anticipated  disturbances 
becomes  critical.  For  systems  that  can  anticipate  time-correlated  (at  least  relative  to 
the  system  dynamics),  or  non-zero- mean  disturbances,  some  sort  of  integral  control 
is  required  to  achieve  near-optimum  performance. 

6.3.2  Error  Integration  through  the  Addition  of  Noise  Estimates 
into  the  System  Dynamics. 

For  this  simple  case  study,  adding  feedback  was  straightforward,  and  an  inner  PI 
error  loop  around  the  planned  and  actual  state  paths  was  included.  For  more  complex, 
highly  non-linear  systems,  this  technique  may  not  be  feasible.  This  is  especially  the 
case  for  systems  with  large  deviations  from  the  planned  path  as  a  result  of  a  high  ratio 
of  disturbances  to  control  authority,  or  systems  with  severe  non-linearities  that  would 
require,  for  example,  an  inordinate  amount  of  gain  scheduling.  A  better  method  is 
to  recognize  that  if  you  applied  the  optimal  control  and  did  not  follow  the  expected 
optimal  path,  the  dynamics  of  the  model  are  not  correct.  Allowance  for  estimated 
error  parameters  found  through  path  error  integration  can  be  added  into  the  dynamics 
for  the  next  epoch,  in  an  effort  to  answer  the  “right”  question. 

For  this  application,  this  would  involve  first  using  the  path  error  to  form  an 
estimate  for  the  wind  bias,  w(t),  and  then  updating  the  dynamics  equation  for  each 
recursive  solution  to  include  the  current  estimate  for  the  wind.  For  more  complicated 
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scenarios,  the  effects  of  the  disturbances  on  the  dynamics  could  be  estimated  through 
both  proportional  and  integrated  error  elements.  For  this  simple  case  study,  however, 
the  optimal  control  estimate,  0*,  can  be  solved  analytically: 


=  tan  1 


{  VacJ  Vacitk)  \ 
X'X'ac-f  %ac{t,k) ) 


Vac\  / 1  + 


Vac.f  Vacitk) 


%ac_f  •^acifk) 


(126) 


Again,  this  is  an  instantaneous  solution  at  any  time,  t^,  used  by  substituting 


the  current  state  into  the  original  problem  as  new  initial  conditions.  The  effects  are 
shown  in  Figure  |30j  Note  that  no  attempt  is  made  to  return  to  any  previous  reference 


Figure  30.  Recursive  Optimal  Control  using  Feedback  to  Update  Dynamics 


solution,  but  instead  the  system  follows  the  optimal  path  that  was  calculated  from 
each  current  position.  Since  the  only  disturbance  that  was  added  was  constant,  linear, 
and  no  measurement  noise  was  considered,  the  estimate  is  correct  after  only  one  time 
step.  Beyond  that,  the  calculated  solution  matches  the  true  optimal  solution  from 
that  point,  since  all  of  the  information  about  the  disturbance  is  completely  known. 

Even  in  a  realistic  environment  where  the  disturbances  are  changing,  this  final 
control  structure  represents  the  best  of  all  worlds,  combining  the  positive  aspects  of 
classical  control  with  the  emerging  benefits  of  real-time  optimal  control.  The  control 
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to  be  applied  is  completely  generated  by  the  numerical  optimization  scheme,  but 
implicitly  contains  the  integrated  error  feedback,  which  is  used  to  update  the  system 
dynamics  and  change  the  optimal  control  problem  for  each  iteration,  overcoming 
the  inability  of  the  purely  recursive  open-loop  structure  to  handle  time-correlated  or 
non-zero  mean  unmodeled  effects. 


6.4  Case  Study  B:  Real-Time  Aircraft  Attack  Planning 

A  more  robust  and  realistic  example  quickly  shows  the  potential  impacts  of  a 
failure  to  consider  error  integration  in  recursive  real-time  optimal  control.  One  of  the 
most  obvious  applications  for  optimal  path  planning  is  for  threat  avoidance.  Stealth 
considerations  of  radar  cross  section,  threat  radar  detection  capability,  and  effec¬ 
tive  surface-to-air  missile  (SAM)  engagement  ranges  must  be  considered  in  attack 
planning.  For  maximum  effectiveness,  the  plan  should  be  accomplished  in  real  time. 
Pop-up  threats,  by  definition  unanticipated,  cannot  be  avoided  using  mission  planning 
that  was  accomplished  prior  to  take-off.  In  addition,  without  the  ability  to  change 
the  plan  enroute,  a  pilot  cannot  immediately  exploit  weaknesses  such  as  a  defense 
system  that  has  been  removed  or  reduced  in  operational  capability  in  some  manner 
by  another  strike  package.  All  of  this  is  possible  with  RTOC. 

Consider  a  strike  planned  on  a  soft  target,  defended  with  a  perimeter  of  SAM 
threats  along  the  planned  route.  For  specific  applications,  the  performance  index 
would  be  designed  to  consider  the  specific  capabilities  of  each  threat  and  the  advan¬ 
tages  of  the  attacking  aircraft,  but  as  a  generic  illustration,  a  15  nm  (nautical  mile)  or 
30  nm  ring  is  assigned  to  each  SAM  location,  and  the  run  is  constrained  to  a  constant 
altitude  with  a  constant  350  knots  true  air  speed  (TAS).  The  SAM  ring  represents 
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a  weapon  employment  zone,  outside  of  which  the  aircraft  can  safely  operate  in  the 
absence  of  air-to-air  threats. 


Admittedly  avoiding  minimum  exposure  and  stealth  issues  (which  could  be  incor¬ 
porated  with  the  appropriate  modeling),  the  basis  for  the  performance  index  for  this 
attack  scenario  remains  a  Mayer  cost  function  of  final  time,  as  it  was  for  the  case 
study  in  Equation  |1 13  This  equates  to  a  minimum  fuel  consumption  index  for  a 
constant  altitude  and  airspeed  run  (if  throttle  increases  during  turns  are  considered 
negligible).  Other  options  could  include  a  penalty  for  proximity  to  threats,  if  flight 
was  allowed  within  the  threat  rings.  In  practice,  non-stealth  aircraft  pilots  determine 
a  safe  distance  from  SAMs  and  stick  to  it,  unless  threat  ring  penetration  is  required. 


The  no-wind  dynamics  remain  unchanged  from  Equation  112,  and  the  control 
is  still  a  commanded  heading,  which  would  be  the  input  to  a  standard  heading-hold 
autopilot  with  a  feedback-based  bank  angle  control  law  to  drive  the  physical  actuators. 
RTOC  provides  the  flexibility  of  avoiding  additional  pop-up  threats  simply  by  adding 
new  path  constraints,  ensuring  flight  outside  of  the  SAM  threat  rings: 


[x[t)  -  Xi}2  +  [y{t)  -  Vi}2  >  Pi  i  =  1 . . .  usam 


(127) 


where  Usam  is  the  number  of  currently  known  SAMs. 

Control  is  accomplished  by  recursively  solving  the  optimal  control  problem,  with 
no  explicit  feedback  (as  before,  some  implicit  feedback  is  available  through  the  re¬ 
initializing  of  the  optimal  control  problem  at  the  current  measured  position).  As 
previously  stated,  this  mirrors  the  structure  of  RTOC  becoming  popular  in  the  liter¬ 
ature,  eliminating  the  inner  feedback  loop  around  the  optimal  path. 

The  optimal  control  solution  is  found  using  a  direct  technique  of  the  class  of  pseu- 
dospectral  methods  known  as  the  Gaussian  Pseudospectral  Method,  which  differs 
slightly  from  the  Radau  method  that  was  used  for  the  quadrotor  flights.  The  method 
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and  software  used  for  this  simulation  are  described  in  |H01  US]-  With  this  efficient 
method,  computation  time  for  each  epoch  took  an  average  of  0.18  seconds  using  30 
nodes  (conservative  for  such  an  application)  on  a  standard  desktop  2.49  GHz  processor 
with  Microsoft  Windows®  XP  running  a  Matlab®  environment.  Increases  in  speed 
could  be  expected  if  the  software  were  tailored  for  this  specific  application  and  the  al¬ 
gorithm  translated  into  a  faster  programming  language  such  as  C++™.  Considering 
the  scenario,  however,  this  is  more  than  adequate  for  real-time  control.  Furthermore, 
in  order  to  clearly  refute  the  point  about  open-loop  recursion  equating  closed-loop 
feedback  if  done  “fast  enough,”  the  simulation  was  artificially  accomplished  with  zero 
computation  time.  Though  this  is  unrealistic,  it  puts  the  recursive  solution  in  the 
best  possible  light,  showing  the  limitations  of  what  could  be  accomplished  even  as 
the  optimal  control  problem  approaches  being  solved  in  real-time.  Any  limitations 
remaining,  therefore,  are  deficiencies  in  the  technique,  and  not  complications  from 
computational  delay  between  the  request  and  the  receipt  of  the  optimal  solution. 


6-4- 1  Pop-up  SAM  Avoidance  Results,  No  Wind  Condition. 


Figure  31 1  shows  the  initial  optimal  path,  planned  by  the  subject  aircraft  as  it 
starts  an  attack  run,  avoiding  the  known  SAM  rings  and  proceeding  to  the  target.  In 
a  deterministic  system,  with  the  absence  of  disturbances  such  as  wind  or  any  further 
threat  information,  this  route  would  be  flown  perfectly,  and  according  to  Bellman’s 
principle  of  optimality,  even  as  the  optimal  problem  is  recalculated  along  the  route 
of  flight,  the  resultant  course  will  never  change  [66] . 

Introduction  of  new  information  is  incorporated  and  adjusted  by  adding  the  ap¬ 
propriate  constraints.  Figure  [3l}j  shows  the  position  of  the  aircraft  along  the  optimal 
path  as  the  aircraft’s  systems  become  aware  of  a  new  emitter  and  the  path  must  be 
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altered.  Solutions  are  continually  being  reproduced  in  the  path  planner,  only  this 
time  the  constraints  will  have  changed. 


(a)  Initial  Flight  Path  (b)  Time=25  min,  as  a  Pop-Up  Threat  Emerges 

Figure  31.  Recursive  Optimal  Path  Planning  Around  Surface-to-Air  Threats — No 
Wind 

Though  direct  methods  are  relatively  insensitive  to  initial  guesses,  optimization 
via  any  gradient  method  is  only  guaranteed  to  find  local  minimums.  For  this  sce¬ 
nario,  any  path  around  each  side  of  every  “wall”  of  contiguous  threats  will  produce 
a  local  minimum,  resulting  in  a  non-convex  space  of  convex  channels.  Several  guess 
generating  algorithms  can  be  designed  to  determine  the  possible  channels  for  in¬ 
vestigation  for  the  global  minimum,  such  as  the  branch-and-bound  technique  found 
in  m  Intelligent  planning  can  be  also  be  used  to  decrease  the  number  of  options. 
Potential  methods  include  dynamic  programming  concepts,  starting  from  the  end  of 
the  solution  and  working  backwards — once  a  global  solution  has  been  found  to  com¬ 
pletion  from  any  point,  there  is  no  need  to  search  that  portion  of  the  path  again. 
Another,  simpler,  solution  for  the  minimum  time  problem  is  just  to  sort  the  channels 
by  distance,  checking  the  shortest  channel  for  feasibility  of  the  optimal  solution  (with 
respect  to  turn  rate  and  other  constraints).  When  the  final  optimal  time  of  the  first 
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feasible  channel  is  less  than  the  minimum  possible  time  in  the  remaining  channels, 
the  search  is  complete.  This  brute  force  method  is  neither  elegant  nor  efficient,  but 
a  better  solution  is  beyond  the  intent  of  this  case  study. 


Figure  32  r  shows  the  result  of  two  new  emitters  being  sensed  by  the  aircraft. 
The  guess-generation  algorithm  provides  two  routes  to  investigate,  and  after  running 
the  optimization  routine  on  the  shorter,  the  longer  route  is  discarded  since  the  min¬ 
imum  possible  time  is  greater  than  the  time  of  the  feasible  solution,  resulting  in  the 
completed  flight  path  of  Figure  [32)o. 


(a)  Time=30  min,  with  New  Pop-Up  Threats.  Ma¬ 
jor  Adjustment  Required 


(b)  Completed  Flight  Path 


Figure  32.  Completion  of  Recursive  Optimal  Path  Planning  Around  Pop-up  Surface- 
to-Air  Threats — No  Wind 


These  results  support  the  efficacy  of  using  an  optimal  control  solution  in  defining 
flight  paths  which  include  changing  parameters  or  constraints,  and  are  on  the  level 


with  the  kind  of  RTOC  simulations  solved  by  the  authors  quoted  in  Section  6T  The 
difficulty  arises  when  stochastic  inputs  in  the  form  of  disturbances  and  measurement 
noises  are  considered.  As  demonstrated  in  the  simple  case  of  Case  Study  A,  the 
controller  will  still  achieve  the  primary  goal,  however,  the  path  taken  may  be  far 
less  than  the  best  that  could  be  accomplished  in  the  same  circumstances,  and  may 
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still  result  in  mission  failure.  For  the  design  of  an  RTOC  system,  additional  control 
may  likely  be  desired  to  overcome  the  lack  of  path-error  integration  in  the  recursive- 
only  structure,  especially  in  the  presence  of  possible  non-zero  mean  or  time-correlated 
disturbances  or  measurement  errors. 


6-4-2  Effect  of  Non- Zero  Mean  or  Time  Correlated  Stochastic  Dis¬ 
turbances. 


A  first-order  Gauss-Markov  process  is  used  to  simulate  potential  wind  gust  inten¬ 
sity: 

gustif )  Tp^Vgustif)  "F  hgustif)  (128) 

where  T  is  a  time  constant  for  the  system,  and  r]gust  is  zero-mean,  white,  Gaussian 
noise  with  E[r)gust(t)]  =  0  and  E[r)gUSt(t)r)gUSt(t  +  r)]  =  Q9USth(r),  using  the  standard 
definition  for  the  delta  function.  Similar  Gauss-Markov  processes  were  used  to  de¬ 
termine  a  lower  frequency  variation  in  wind  intensity,  wpred_windi  and  for  determining 
variance  in  the  wind  direction.  Measuring  wind  velocity  in  knots,  and  direction  in 
degrees,  the  time  constants  for  the  two  wind  components  were  200  hrs  and  40  hrs, 
with  respective  input  strengths  of  0.25  and  0.2  knots2,  and  wind  direction  was  deter¬ 
mined  with  a  time  constant  of  50  hours  and  unit  intensity  noise.  The  resulting  wind 
intensity  and  direction  were  added  to  a  predominant  wind  and  predominant  direction 
biases,  respectively,  resulting  in  the  disturbance  input  shown  in  Figure  [33]  This  is 
representative  of  a  weather  forecast  for  winds  220°  variable  230°  at  30  gusting  35 
knots  (or  an  average  summer  day  at  altitude). 

As  the  final  time  was  unknown,  a  longer  time  history  of  wind  was  generated  than 
was  actually  used.  The  wind  disturbance  causes  the  same  steering  difficulty  shown 
in  Figure  [27]  for  Case  Study  A.  No  matter  how  fast  a  recursive  optimal  solution  is 
calculated,  without  a  position  feedback  loop  to  directly  compensate,  or  feedback  in 
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Wind  Intensity 


Time  (hrs) 

Figure  33.  Wind  Disturbance  Added  to  the  System 


the  form  of  a  wind  estimate  term  from  integration  of  path  error  being  fed  to  the 
optimal  control  problem,  the  unmodeled  wind  will  always  result  in  a  steering  error 
between  the  calculated  solution  and  that  which  would  be  truly  optimal.  Once  near 
the  SAM  rings,  the  errors  in  steering  become  more  critical  and  a  constraint  is  violated, 


as  shown  in  the  inset  of  Figure  34  The  magnitude  of  the  constraint  violation  is  a 


Figure  34.  Steering  Failure  with  Recursive  RTOC  Control  Structure  in  the  Presence 
of  Wind 


function  of  the  size  of  the  wind  disturbance,  the  recursive  solution  update  timing, 
and  any  applied  turn  rate  limit.  If  the  aircraft  is  allowed  an  infinite  turn  rate,  the 
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recursive  system  will  always  meet  the  constraint  as  the  update  interval  approaches 
zero  (this  assumes  the  vehicle  is  riding  the  “outside”  of  a  constraint  that  is  curved 
away  and  does  not  necessarily  hold  for  attempts  to  ride  the  inside  of  a  curve). 

For  this  scenario,  minor  deviations  will  likely  not  mean  the  difference  between 
life  and  death,  but  there  certainly  are  systems  with  hard  limits  (physical  terrain, 
structures,  etc.),  and  optimal  solutions  often  ride  as  close  as  allowable  to  those  limits. 
If  the  ability  of  the  system  to  change  course  is  limited  (i.e.  a  slow  maximum  turn  rate), 
then  late  steering  corrections  approaching  a  constraint  can  cause  large  violations. 

Besides  potential  violations,  the  main  point  of  the  exercise  is  to  show  that  the 
path  itself  is  clearly  not  optimal.  Recall  from  Case  Study  A  that  there  will  always  be 
steering  error  in  the  case  of  a  time-correlated  or  non-zero  mean  disturbance.  This  can 
be  seen  in  the  bending  of  the  optimal  path  of  Figure  [34|  just  as  was  the  case  for  the 


showed  that  the  steering  error  was  not  a  function  of  the  update  timing,  but  of  the 
problem  geometry  and  the  magnitude  of  the  disturbance.  This  is  why  faster  updates 
did  not  remove  the  problem,  as  illustrated  in  Figure  |27]a.  Increasing  the  update  rate 
does  decrease  the  amount  of  time  that  you  follow  the  erroneous  heading,  but  there 
will  only  be  small  changes  in  the  erroneous  heading  command  for  the  next  step  until 
there  is  significant  deviation  from  the  optimal  path,  when  it  is  too  late. 

6-4-3  Integration  of  Path  Error. 

To  correct  the  non-optimal  bending  of  the  path  due  to  the  disturbance  bias,  the 
bias  is  estimated  and  included  in  the  optimal  control  formulation  for  the  next  epoch. 
Note  that,  though  helpful,  it  is  not  required  that  the  source  of  the  bias  even  be  known. 
Path  deviations  may  come  from  poor  sensors,  wind,  poorly  rigged  flight  controls,  or 
other  sources.  As  in  adaptive  control,  applying  the  open-loop  control  and  compar- 


homing  solution  of  Figure ^7)  For  Case  Study  A,  the  analytic  solution  in  Equation  123 
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ing  the  resulting  trajectory  to  the  expected  trajectory  provides  the  opportunity  to 
estimate  parameters  which  may  be  used  to  update  the  model  for  each  epoch. 

For  this  implementation,  estimates  of  the  wind  direction  and  velocity  are  required, 
broken  down  into  components  in  the  x  and  y  directions.  In  the  absence  of  a  direct 
measurement  source,  this  can  be  produced  from  the  difference  between  the  expected 
and  actual  position  in  each  axis  divided  by  the  time  step  (or  an  averaged  position 
over  several  time  steps).  A  simple  estimation  filter  is  used  for  the  demonstration, 
with  the  initial  condition  determined  by  the  first  measurement: 

(f-fc+l)  'UJx  (tk)  “I”  kw ind  [®imeos  (tfc)  'UJx  (tfc)]  (129) 

An  identical  formulation  is  used  for  the  y-axis  component.  For  simplicity,  one  tenth 
of  the  residual  error  is  applied  at  each  time  step  ( kwinfj  =  0.1),  but  the  Kalman  filter 
equations  could  easily  be  implemented  for  a  more  optimal  choice  for  kWind- 

With  an  available  wind  estimate  generated  from  the  closed-loop  feedback  of  the 
vehicle  state,  the  assumed  system  dynamics  are  updated  by  adding  the  appropriate 
components  into  each  channel  and  the  recursion  is  allowed  to  proceed.  Using  decision 
points  similar  to  those  from  Figure  [32|j,  where  the  aircraft  is  made  aware  of  pop¬ 
up  SAM  threats,  the  completed  flight  path  can  be  seen  in  Figure  [35j  and  is  almost 
indistinguishable  from  the  no- wind  optimal  path.  The  mission  is  accomplished  in  the 
presence  of  changing  threats  and  non-zero  mean  disturbances. 


6.5  Recommended  RTOC  Structure 

Both  case  studies  have  shown  the  detrimental  effects  of  implementing  RTOC  in 
a  purely  fast  open-loop  recursive  scheme.  The  current  trend  in  RTOC  algorithms 
has  been  to  use  recent  computational  speed  increases  to  implement  a  purely  feed- 
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Figure  35.  Complete  Flight  Path,  Wind  Compensated  for  through  Estimation  from 
Position  Feedback 

forward  system  with  instantaneous  optimal  solutions  only.  This  eliminates  the  use  of 
a  traditional  inner-loop  to  maintain  the  optimal  path  in  the  presence  of  disturbances 
in  favor  of  merely  replacing  the  optimal  path  entirely.  Though  this  can  be  effective  in 
simulation,  this  method  is  by  no  means  optimal,  and  it  suffers  greatly  in  the  presence 
of  stochastic  inputs — particularly  those  which  are  non-zero  mean  or  time-correlated. 

Individual  control  problems  will  always  require  a  designer’s  eye  for  the  best  control 
structure  for  a  particular  purpose,  but  no  matter  what  method  of  control  is  selected, 
the  integration  of  past  error  between  the  expected  and  actual  trajectories  must  be 
included  in  the  determination  of  future  control.  For  systems  guided  with  RTOC 
to  handle  changing  environments  (such  as  pop-up  SAMs),  a  classical  inner- feedback 
loop  is  still  required  for  steady-state  performance.  The  inner-loop  error  signal  is 
added  to  the  optimal  control  to  maintain  the  optimal  trajectory  in  the  presence  of 
unmodeled  effects  and  non-zero  mean  or  time-correlated  disturbances.  When  possible, 
an  additional  method  includes  both  this  inner  loop,  and  feeding  back  disturbance 
estimates  into  the  optimal  control  problem,  changing  the  dynamics  equations  in  each 
epoch  to  make  the  model  best  match  reality. 
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VII.  RTOC  Algorithm  and  Implementation  Tools 


£  I  HIS  chapter  addresses  the  algorithm  employed  for  the  real-time  optimal  control 
portions  of  the  research,  detailing  both  the  framework  of  the  RTOC  imple¬ 
mentation,  and  the  optimal  control  solution  algorithm  itself.  Completion  of  the  design 
process  through  actual  hardware  implementation  and  subsystem  integration  brought 
out  several  key  implementation  lessons  that  will  be  useful  to  future  RTOC  designers. 


7.1  RTOC  Algorithm 


Figure  [36]provicles  the  essential  decision  outline  for  three  control  segments  required 
to  land  the  quadrotor  on  a  power  line.  For  more  specifics,  the  top  level  shell  of  the 
Matlab®  code  to  execute  this  loop  is  provided  in  Appendix  [Bj  The  acquisition 
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Figure  36.  RTOC  Algorithm  Structure 
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segment  is  completed  when  the  power  line  is  identified  by  the  sensor,  and  an  initial 
target  estimate  and  trajectory  are  initiated.  For  the  flight  test,  a  “shell”  was  created 
with  a  list  of  commands  to  the  quadrotor  to  takeoff,  stabilize,  and  move  to  a  hover 
position  until  the  first  measurement  was  received  or  a  timeout  occurred,  at  which  time 
the  aircraft  landed.  For  both  the  quadrotor  and  the  full  power  line  scenario,  since  the 
initial  target  estimate  and  covariance  are  provided  as  a  guess  to  the  UKF  (based  on 
likely  height  of  the  power  line  and  likely  sensor  acquisition  range),  an  initial  trajectory 
can  also  be  pre-calculated  off-line,  and  used  to  seed  the  trajectory  planner’s  initial 
guess.  This  is  not  required,  since  direct  methods  are  tolerant  of  poor  initial  guesses, 
but  it  sets  up  the  system  for  a  fast  first  solution.  After  the  first  pass  of  the  trajectory 
solver,  the  previous  epoch’s  solution  is  always  used  for  the  initial  guess,  trimming  off 
the  initial  portion  that  should  have  already  been  flown.  Once  the  approach  segment’s 
main  loop  is  entered,  it  is  executed  until  the  vehicle  reaches  the  approach  point  with 
the  required  certainty  in  the  target  location,  at  which  point  the  aircraft  enters  the 
flare  segment  to  land. 

The  heart  of  the  approach  segment  is  the  iterative  RTOC  algorithm.  As  the 
recursive  estimation  filter  provides  updated  target  coordinates,  the  estimate  for  the 
required  approach  point,  xapp,  is  updated,  and  the  trajectory  planner  then  calculates 
an  update  to  the  optimal  path.  Each  solution  is  a  control  state  pair,  {x)i(f),  u£(£)}, 
t  G  [tk,tf],  that  is  semi-discrete — every  epoch  contains  the  complete  state  and  control 
time  history  for  the  remainder  of  the  flight.  Non-optimal  portions  of  the  path  are 
spliced  onto  the  path  as  well.  These  commands  give  the  vehicle  a  “missed  approach” 
plan  for  what  to  do  if  the  exit  criteria  for  the  approach  segment  are  not  achieved. 
This  would  be  especially  significant  for  times  when  the  measurement  data  is  lost  for 
a  significant  length  of  time.  For  short  periods  with  no  new  data,  the  plan  will  simply 
be  updated  to  initialize  with  a  higher  than  expected  covariance  than  was  planned  for 
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in  the  previous  epoch.  The  quadrotor’s  missed  approach  plan  consisted  of  a  simple 
landing  profile.  For  the  full  sUAS,  it  would  likely  include  circling  back  to  the  location 
of  the  last  known  good  measurement,  with  a  further  contingency  plan  after  that. 

Once  the  main  RTOC  loop  of  the  approach  segment  is  entered,  note  that  the  call 
to  the  UKF  counter-intuitively  happens  after  the  trajectory  planner.  The  trajectory 
planner  consumes  most  of  the  loop  time,  A tcaic.  With  a  slow  sensor  update  rate, 
it  is  not  likely  that  measurements  will  arrive  between  the  time  the  UKF  provides 
an  estimate  and  the  time  the  trajectory  planner  begins  calculations  on  the  next 
epoch.  During  the  trajectory  planner  calculations,  however,  multiple  measurements 
will  likely  be  received,  and  the  target  estimate — and  thus  xapp — should  incorporate 
the  new  measurement  data  prior  to  checking  to  see  if  the  approach  point  has  truly 
been  achieved  and  the  required  certainty  has  been  met. 

7.1.1  Initial  Condition  Validity. 

An  easily  overlooked,  but  critical,  consideration  must  be  taken  with  respect  to 
initial  conditions.  It  was  outlined  in  Section  15.3.31  that  the  initial  conditions  for  each 
trajectory  planning  epoch  are  set  based  on  the  expected  future  conditions  at  the  time 
the  solution  is  planned  to  be  available.  The  initial  condition  x$k  x  =  x(t/,  +  A tcaic)  is 
based  on  the  optimal  time  history  x*,,  which  was  solved  relative  to  the  target  estimate 
x4fc .  Note  also  that  many  of  the  constraints  on  the  optimal  solution  are  also  set  relative 
to  the  target  estimate,  such  as  the  constraint  to  stay  within  an  area  where  the  target 
will  be  seen  in  the  fixed  camera  FOV.  Since  it  is  derived  from  the  optimal  solution, 
x0*.+1  will  always  reside  within  the  constraints,  at  least  as  well  as  they  were  known  to 
be  for  epoch  k.  However,  as  the  algorithm  of  Figure  [36]  progresses,  the  condition  can 
occur  (and  often  does,  since  optimal  solutions  tend  to  “ride”  on  constraints),  that 
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when  the  target  estimate  is  updated  to  xtfc  and  the  relative  boundaries  move,  the 
initial  condition  may  rest  outside  of  the  boundaries  for  that  epoch. 


A  processing  step  must  be  made  at  every  epoch  to  check  all  of  the  initial  conditions 
for  validity,  else  the  trajectory  planner  will  never  converge  to  a  feasible  solution.  For 
the  quadrotor  algorithm,  invalid  initial  conditions  were  moved  to  the  closest  point  in 
the  most  current  valid  flight  envelope.  This  may  result  in  a  discontinuity  between 
the  present  position  and  the  next  commanded  position.  A  smoothing  function  can  be 


applied  as  will  be  developed  in  Section  |7.1.3|  to  mitigate  difficulties  caused  by  using 
a  variable  calculation  time. 


7.1.2  Variable  Calculation  Time. 

For  simplicity  of  process  integration,  researchers  working  in  RTOC  typically  choose 
to  update  the  optimal  solution  at  a  fixed  loop  time,  A tcaic.  This  allows  the  flight  con¬ 
trol  algorithm  to  look  for  a  new  optimal  solution  at  a  set  time  in  the  flight  control 
loop.  The  downside  to  this  approach  is  that  the  trajectory  planner  must  be  finished 
prior  to  that  time,  and  the  calculation  time  can  vary  greatly.  A  very  conservative 
A  tcaic  must  be  selected,  and  efficiency  is  sacrificed  as  every  iteration,  by  design,  takes 
the  maximum  allowable  iteration  time.  Allowing  the  loop  time  to  be  variable  increases 
the  rate  of  receiving  optimal  path  updates.  The  downsides  are  coding  complexity  for 
timing  transitions,  and  the  fact  that  the  projected  initial  conditions  may  not  match 
the  current  commanded  conditions  at  the  new  epoch. 

A  variable  calculation  time  was  used  for  this  research,  and  A tcaic  was  set  as  the 
expected  calculation  time,  vice  the  maximum.  The  efforts  of  the  flight  control  au¬ 
topilot  and  the  optimization  software  were  processed  independently,  but  threaded 
together  to  allow  the  optimal  solution  to  be  applied  as  soon  as  it  was  available.  As  an 
engineering  safety  valve,  maximum  iteration  limits  were  still  set  for  the  optimization 
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software,  but  they  were  not  triggered  in  the  tests  conducted  once  the  problem  and 
constraint  formulations  were  finalized.  The  concept  was  that  if  the  optimal  solver 
was  unable  to  converge  on  a  particular  instantiation  of  the  optimal  problem,  it  would 
be  reset  with  the  current  conditions  and  target  estimate,  throwing  out  the  previous 
solution  as  its  initial  guess. 

Using  a  variable  calculation  time  method  could  potentially  impact  the  application 
of  optimal  solutions  that  are  not  available  until  after  the  expected  amount  of  calcu¬ 
lation  time.  For  solutions  that  are  available  (tk+i)  earlier  than  expected  (£ofe+i),  the 
new  portion  of  the  optimal  solution  is  simply  appended  to  the  discrete  path  and  the 
effects  are  transparent: 

If  tk- 1-1  ^Ofc_|_i  tk  T  Afca/c, 

Xfe+i(t)  =  {x*k[tk,tk  +  A tcalc  ~  A t],  X*k+1[tk  +  A tealctf}}  (130) 

For  solutions  that  are  available  later  than  expected,  the  implication  is  that  a 
discontinuity  is  possible  in  the  state  and  control  at  time  tk+\.  The  error  between  the 
actual  state  and  the  planned  state  as  each  old  solution  is  replaced  is  now  a  factor 
not  only  of  how  close  the  vehicle  tracks  the  planned  state,  but  also  depends  on  the 
distance  and  direction  the  vehicle  has  traveled  in  the  amount  of  time  the  calculation 
took  beyond  that  which  was  expected.  If  the  calculation  took  significantly  longer 
than  expected,  this  discontinuity  could  be  significant. 

A  similar  discontinuity  can  occur  at  the  approach  point.  While  the  trajectory 
planner  is  calculating,  new  measurements  are  still  being  received.  Considering  this 
information,  the  estimate  of  the  target  location  will  likely  have  moved  while  the 
trajectory  is  being  applied.  The  unfortunate  cumulative  effect  is  that  by  the  time  an 
optimal  solution  becomes  available,  it  travels  from  a  place  the  vehicle  is  no  longer  at, 
to  a  place  the  target  estimate  is  no  longer  at.  This  cannot  be  solely  controlled  by 
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increasing  the  recursion  timing,  as  the  target  estimate  moves  in  instantaneous  steps 
as  measurements  come  in.  A  blending  strategy  ensures  smooth,  continuous  control 
and  adds  corrections  to  the  path  ends. 

7.1.3  Correction  Blending  of  Path  Ends. 

There  are  optimal  methods  for  resolving  the  differences  in  initial  and  final  con¬ 
ditions,  most  notably  those  of  neighboring  optimal  control  (NOC)  |lTS1  1119].  For 
systems  where  these  differences  are  critical,  NOC  is  recommended.  Experimentation 
with  this  system  suggested  that  the  differences  in  initial  conditions  were  very  small 
(as  it  will  be  for  systems  where  the  calculation  time  is  fairly  predictable).  During 
flight  test,  the  longest  calculation  time  was  only  0.11  seconds  beyond  what  was  an¬ 
ticipated,  leading  to  very  small  initial  discontinuities.  Changes  at  the  “tail”  of  the 
path  can  be  substantial,  depending  on  how  far  the  target  estimate  moves  during  each 
measurement  update. 

Stability  for  the  quadrotor  system  in  the  face  of  a  discontinuity  in  commanded 
trajectory  was  never  a  question,  as  the  autopilot  was  designed  with  velocity  limits  to 
be  stable  for  any  size  command  step.  The  tail  of  the  path  was  certainly  more  sensitive 
to  measurement  updates,  but  until  the  end-game,  the  tail  portion  of  the  path  will 
be  replaced  each  epoch  before  it  is  actually  flown.  As  a  result,  the  computational 
expense  of  NOC  was  forgone  for  a  simple  and  efficient  strategy  that  ensured  the  path 
would  always  end  at  the  most  current  target  estimate,  but  without  discernible  delay. 
This  correction  is  critical  for  the  last  seconds  of  the  flight,  but  is  a  nice  feature  for 
robustness  as  well,  as  the  path  “in  hand”  is  always  based  on  the  best  information 
at  the  time,  and  is  the  best  plan  to  follow  in  the  case  of  mechanical  failure  of  the 
optimal  solver,  or  a  delay  caused  by  an  inability  to  converge  on  a  solution. 
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The  initial  condition  discontinuities  can  occur  when  the  optimal  solution  for  epoch 


k  +  1,  available  at  tk+i,  arrives  later  than  the  expected  time  of  tok+1.  Until  tk+i,  the 
system  continues  to  fly  the  solution  that  was  produced  for  epoch  k.  The  final  condition 
discontinuities  occur  when  the  trajectory  planner  delivers  a  path  for  epoch  k  +  1  to 
the  assumed  target,  ,  that  has  been  updated  by  the  estimation  algorithm  to  x^ 
during  the  calculation  time  of  the  path  planner.  Sample  results  of  the  blending  can 


be  seen  in  Figure  37,  where  the  dark  black  line  indicates  the  path  that  is  sent  to 
the  vehicle  at  the  actual  update  time  tk+i-  The  path  sent  is  a  composite  of  the  solid 
optimal  solution  at  xjl,  the  dashed  solution  at  x£+1,  and  the  blending  correction  as  a 
result  of  updating  the  target  to  x^  during  calculation  time. 


x(m) 


Figure  37.  Cosine  Blending  Corrections 


To  produce  the  blending  without  generating  the  sharp  changes  of  trajectory  with  a 
linear  blending  method,  a  cosine  wave  was  used  to  “round  the  corners”  and  smoothly 
transition  the  head  or  tail  of  the  path  to  the  corrected  point.  The  calculations  are 
described  here  for  the  tail  of  the  path  with  a  discrete  time  series  vector,  as  it  is 
actually  applied  in  the  physical  system.  The  length  of  the  blending  segment,  tbimax, 
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is  selected  by  the  desired  segment  time,  tseg,  limited  to  the  amount  of  time  remaining 
if  the  path  is  already  within  the  final  window: 

t bimax  =  min  [tseg,tf  -tk-  rem  (tf  -  tk,  At)]  (131) 

The  remainder  function  (rem)  is  used  to  ensure  an  even  division  by  At  (tseg  is 
chosen  this  way  as  well).  In  practice,  it  was  found  that  blending  initial  differences 
(if  they  exist)  over  one  second,  and  final  differences  over  5  seconds  was  efficient  and 
effective.  A  time  vector  is  then  produced: 

tbl  =  [0,At,2At,...,tblmaf  (132) 


For  corrections  at  the  tail  of  the  path,  the  point  at  which  the  new  path  departs 
from  the  old  should  be  smooth.  A  correction  wave  vector  from  zero  to  one  with  a 
slow  initial  transition  is  created  using  one-quarter  of  a  cosine  wave  period,  and  is 
directionally  scaled  by  the  amount  the  target  estimate  was  moved  in  each  state  at 
the  last  batch  update  to  create  a  correction  matrix: 


o— >i  =  1  -  cos  (7rtw/2iWmoJ 
=  S0_>r(x+  -xt-)T 


(133) 


m,cork 


The  correction  matrix  is  then  used  to  update  the  path  segment: 


\tapp  tbi  max  ?  tapp]  [t. 


app 


t  bimax  1  tappl  + 


cork 


(134) 


When  this  technique  is  used  to  correct  discontinuities  in  initial  conditions,  the 
error  to  be  rectified  is  measured  from  beyond  the  moment  when  the  new  path  becomes 
available,  at  tk+ 1  +  tbimax ■  During  the  flare  segment  of  the  flight,  a  similar  technique  is 
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also  used  to  generate  a  horizontal  profile,  providing  a  smooth  path  from  the  approach 
point  to  the  point  where  the  vehicle  actually  hooks  the  wire.  In  both  of  these  cases, 
both  ends  of  the  blend  are  desired  to  be  smooth,  so  a  correction  wave  from  zero  to 


one  such  as  the  one  in  Equation  133  is  used  with  a  higher  frequency  (one-half  of  a 
full  cosine  wave).  For  the  initial  condition  blending,  this  allows  both  the  initial  move 
from  the  old  path  and  the  blend  into  the  new  path  to  both  have  smooth  transitions: 


E0^1flare  =  V2  -  V2COS  (TTtb/tblmax) 

“ COrflare  =  ^0^-1  flare  ““  X <W 

7. 1.3.1  Process  Threading. 

The  last  noteworthy  implementation  lesson  came  from  timing  synchronization 
problems  stemming  from  using  a  flexible  calculation  time  for  the  optimal  path  planner 
on  actual  hardware.  The  UKF,  trajectory  planner,  communication  paths,  autopilot 
processes,  and  speed  control  servos  are  each  running  iterative  loops,  but  all  at  different 
rates.  Threading  loops  with  known  rates  is  not  difficult,  but  the  trajectory  planner 
has  a  variable  cycle  time  (just  over  1  Hz  for  this  application).  Working  at  a  much 
faster  rate  (50  Hz),  the  autopilot  must  have  a  buffer  of  future  commands  to  process, 
and  a  “dealer”  function  was  implemented  as  a  solution  to  run  between  the  programs 
as  a  storage  place  for  each  epoch’s  optimal  path  time  history.  This  allowed  the 
flight  control  and  optimization  algorithms  to  be  carried  out  on  separate  processors, 
and  handled  the  asynchronous  timing  between  them  without  resorting  to  slowing  the 
process  by  saving  the  path  to  a  hie.  A  dealer  function  can  be  run  at  high  speeds, 
checking  for  a  complete  path  update  (the  “deck”  if  you  will)  without  ceasing  to  provide 
a  list  of  commanded  positions  and  heading  at  each  time  step  of  the  autopilot. 

When  a  new  optimal  path  is  formed,  it  is  sent  via  TCP  packets  using  a  blocking 
protocol  to  stop  processing  on  the  low,  variable  rate  processor  until  the  deck  is  picked 
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up.  This  makes  any  delay  less  than  one  time  step  of  the  higher  rate  function  (the 
dealer  runs  at  100  Hz),  and  ensures  the  new  path  can  be  used  as  soon  as  it  is  ready. 
A  similar  method  was  used  in  the  other  direction  to  get  measurements,  limits,  and 
initial  conditions  into  the  RTOC  process,  stopping  processing  after  sending  a  “ready” 
poll,  checked  for  during  each  loop  of  the  dealer.  With  this  technique,  slowing  down 
the  trajectory  planner  to  allow  a  fixed  calculation  time  is  not  necessary. 


7.2  Radau  Pseudospectral  Method 


The  final  area  of  RTOC  implementation  to  address  is  the  actual  solution  method 
for  the  optimal  control  problem.  Pseudospectral  methods  have  the  most  advantageous 
calculation  speed,  and  are  appropriate  given  the  knowledge  that  a  flight  trajectory 
will  be  smooth  and  differentiable.  Adaptive  grid  refinement  techniques  were  applied 
to  allow  segmentation  of  the  problem  in  the  face  of  potential  discontinuities.  An  open- 
source  software  algorithm  known  as  QVOVS  v3.3  was  used  with  the  Radau  Pseu¬ 
dospectral  Method  (traditional  Radau  points,  including  the  initial  point)  to  formulate 
the  continuous  problem  into  an  NLP,  and  the  industry  standard  SNOPT  v7  was  used 
to  solve  it.  Using  open-source  software  allowed  minor  modifications  for  speed  when 
implemented  in  real-time.  The  algorithm  used  is  collected  from  j90j'35,  S||39j[5].  The 
general  concepts  of  transcription  were  introduced  in  Chapter  |TT[  including  transforma¬ 
tion  of  time  to  the  interval  r  €  [—1, 1]  to  make  use  of  Gaussian  quadrature.  On  that 
interval,  collocation  is  performed  at  the  Legendre-Gauss  Radau  points,  which  may 
be  obtained  by  first  producing  the  Legendre  polynomial,  expressed  with  Rodrigues’ 
formula  as: 


Pn(t) 


1  dN 
2NN\  drN 


(136) 


where  N  is  the  number  of  nodes  desired  to  collocate  at. 
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The  actual  collocation  points,  rk,  are  the  roots  of  Pn(t)  +  Pn- i(t),  which  will 
always  contain  the  initial  point,  T\  =  —1,  and  where  Tn  <  1-  The  quadrature  weights 
associated  with  these  points  are  solved  for  off-line  with  an  algorithm  based  on  the 
LGR  Vandermonde  matrix,  and  saved  for  rapid  use  during  the  real-time  application. 
Note  that  for  these  weights,  and  polynomials,  4>p,  of  degree  at  most  2 N  —  2: 

rl  N 

/  0p(t)  dr  =  Yw^n)  (137) 

d  ~l  i=i 

The  discretization  points  include  all  of  the  collocation  points  and  the  end  point, 
7jv-i-i  =  1.  Using  Li,  i  =  1, . . . ,  N  +  1  as  a  basis,  accurate  approximation  of  each  of 
the  nx  states,  x3,  can  be  performed  with  a  polynomial  of  at  most  degree  N: 

N+l 

x3{t)  ~  Y  XijLi(T)  j  =  1,  •  •  • ,  nx  (138) 

i=  1 

The  basis  elements  are  found  using  the  standard  Lagrange  interpolating  polynomial 
definition: 

N+l 

UM=  n  wrv  <139) 

j=l,j¥=i  1  J 

Collocation  will  require  comparing  the  known  derivative  for  each  state  from  the 
system  dynamics  equations  with  the  derivative  of  the  approximating  polynomial  for 
each  state  at  each  collocation  point.  Differentiating  each  state  component,  x3,  at  each 
collocation  point,  rk,  gives: 

N+l  N+l 

Xj{rk)  ~  ^  ^  XijLi(Tk)  ^  ^  DkiXij ,  Dki  Uj(Tfc)  (140) 

i= 1  i— 1 

The  components  are  assembled  into  the  differentiation  matrix,  D  G  M7VxJV+1, 
with  a  row  for  each  collocation  point  and  a  column  for  the  derivatives  of  each  of  the 
N+l  Lagrange  polynomials  evaluated  there.  Note  that  this  matrix  may  be  calculated 
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entirely  off-line  with  only  the  knowledge  of  the  number  of  nodes  to  be  used  in  the 
solution,  allowing  for  extremely  efficient  calculation  of  the  derivative  of  each  state  at 
every  collocation  point  in  aniVx  nx  matrix  that  can  be  written: 


iiW  “  (DXL  i  =  l,...,N  j  = 


(141) 


Since  the  polynomials  for  each  state  are  at  most  degree  N,  the  derivative  approxi¬ 


mation  is  exact.  The  matrix  X  G  MAr+lxna:  is  made  of  the  coefficients  of  Equation  138 
and  includes  row  vectors  of  the  state  components  at  every  discretization  point: 


Xi  =  X(Ti)  = 


Xil  *  *  *  Xi 


i  =  1, . . . ,  N  +  1 


(142) 


The  nu  dimensional  controls  can  also  be  expressed  as  row  vectors  of  all  the  control 
elements  at  a  particular  time,  but  this  is  only  necessary  at  the  collocation  points: 


■yLGR  _ 


Un  •  •  •  'U'in-u 


i  =  1,..., N 


(143) 


To  complete  the  conversion  from  the  continuous  optimal  control  problem  into 
the  static  NLP,  the  dynamic  constraints,  /,  from  Equation  88  on  page  82|  are  ex¬ 
pressed  as  a  matrix  formed  from  the  state  values  at  each  of  the  collocation  points, 
F  (XLGR,  ULGR)  G  RNxn*  such  that: 


Fa  (XLGR,  ULGR)  =  fj  (X^GR,  URGR)  i  =  l,...,N  j  =  l,...,nx  (144) 


where: 


X  = 


-^■LGR 

Xjv+i 


(145) 
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The  NLP  is  then  defined  as  minimizing  the  approximation  of  the  continuous  cost 
function: 

t  _t  N 

jLGR  =  p  (Xo>  to,Xf,tf)  +  WkC(Xk,  U*,  Tk-  t0,tf )  (146) 

k= 1 

The  original  dynamic  constraints  are  now  a  series  of  static  constraints  for  every 
state  at  every  node: 

DX  -  ~  —  F  (XLGR,  ULGR)  =  0  (147) 

with  the  original  constraints  and  boundary  conditions  now  evaluated  discretely  as: 

7  (Xo,  to,  Xjv+i,  tf)  >  0  (148) 

C  (X,j,  Uj,  r*;  t0,  tf)  >  0  i  =  l,...,N  (149) 

7.2.1  Solving  the  NLP. 

The  solver  SNOPT  introduces  slack  variables  to  convert  all  constraints  to  equality 
conditions.  A  modified  Lagrangian  is  formulated  by  augmenting  the  cost  function 
with  Lagrange  multipliers  applied  to  each  constraint,  and  the  optimality  conditions 
are  found  by  taking  the  partials  of  the  Lagrangian  with  respect  to  the  states,  controls, 
and  multipliers  and  setting  them  to  zero.  Though  QVOVS  provides  a  very  effective 
automatic  differentiation  package,  analytic  derivatives  were  used  as  the  most  accurate 
and  efficient  method  of  gradient  determination.  The  trivial  boundary  conditions  are 
omitted,  but  the  remaining  analytical  derivatives  are  summarized  in  Table  [l] 

With  the  modified  Lagrangian,  SNOPT  uses  a  two-tier  iteration.  Simplifying, 
major  iterations  linearize  all  constraints  with  a  truncated  Taylor  series,  evaluating 
the  Jacobian  for  the  constraints  at  the  iterate  point  and  formulating  a  new  subprob¬ 
lem  with  a  quadratic  approximation  of  the  modified  Langrangian  and  the  linearized 
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constraints.  Minor  iterations  solve  each  subproblem  with  a  reduced  Hessian  active- 
set  method.  This  method  seeks  to  reduce  the  computational  expense  of  calculating 
the  Hessian  by  freezing  some  of  the  variables,  and  moving  along  the  feasible  curve 
in  the  direction  of  the  reduced  gradient  to  minimize  the  cost  function.  Reaching  a 
minimum,  more  of  the  variables  are  allowed  to  move.  Upon  reaching  a  solution,  a  La- 
grangian  merit  function  is  formed,  and  a  line  search  along  that  function  is  made  from 
the  subproblem  solution  point  to  a  new  point,  where  the  constraints  are  re-linearized 
and  the  process  continues  until  tolerances  of  the  major  iterations  are  met. 


Table  1.  Non-Zero  Analytic  Derivatives 


Equation 


Non- Zero  Partial  Derivatives 
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X 
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7.2.2  Adaptive  Grid  Refinement. 


Clearly,  with  constraints  defined  for  the  derivative  of  every  state  at  every  node  in 
addition  to  the  typical  constraints  of  an  optimal  control  problem  (boundary,  path, 
event,  etc.),  the  dimensionality  of  the  NLP  increases  greatly  with  the  number  of 
nodes.  Using  a  small  number  of  nodes  provides  a  fast  solution,  but  potentially  at 
the  cost  of  accuracy.  For  this  dissertation,  Darby’s  adaptive  griding,  introduced  in 
Section  2. 2. 2.1,  is  incorporated  [21].  The  total  number  of  nodes  is  divided  into  s 


segments  with  Ns  nodes  in  the  respective  segment: 

5 

n  =  Y,n, 


(150) 


S=1 


Path  constraints  and  boundary  constraints  do  not  change,  but  the  collocated  dynamic 
constraints  must  be  modified  to  reflect  transforming  each  segment  of  times  t  G  [ts~i,  is] 
to  r  G  [—1, 1]: 


0 

...  0 

^Ii  0 

0 

0 

d2 

...  0 

X- 

0  ■■■ 

0 

0 

0  d5 

o 

o 

ts—ts- 1  T 

2  LS  J 

F  =  0  (151) 


Total  cost  now  becomes  a  sum  of  the  segment  costs,  and  continuity  is  ensured 
by  forcing  each  state  to  start  a  segment  with  the  value  it  had  at  the  completion  of 
the  prior  segment.  Formulating  the  problem  in  this  manner  actually  increases  the 
sparsity  of  the  NLP  Jacobian,  resulting  in  less  computational  time. 

The  risk  of  the  method  is  loss  of  spectral  accuracy  with  fewer  nodes  in  each 


segment.  To  check  for  this,  the  collocation  constraints  in  Equation  147  which  are 
mandated  to  be  zero  at  all  of  the  collocation  points,  are  evaluated  in  between  the 
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collocation  points  (ideally,  the  equations  should  be  zero  everywhere,  but  they  are 
only  constrained  at  the  nodes).  Midpoints  between  collocation  points  are  found: 


U 


U  d" 
2 


i  =  1, . . . ,  Ns  -  1 


(152) 


The  states  are  evaluated  at  the  midpoints  using  the  Lagrange  polynomial  approxi¬ 
mations,  and  the  controls  are  approximated  with  cubic  interpolation  at  the  midpoints, 
resulting  in:  X,  U  G  M,Ns~lxn*.  The  differentiation  matrix  is  the  square  Lobatto  ma¬ 
trix,  D  G  M.Ns~1xNs~1  ,  allowing  a  midpoint  residual  matrix  to  be  formed: 


R 


DX  -  ts  +  t^F  (X,  U,  r;  ta_1;  ts) 


JgA's-iXUi 


(153) 


Note  that  |-|  indicates  the  element-wise  absolute  value.  Ideally,  the  residuals  would 
all  be  zero  and  the  dynamic  constraints  would  perfectly  match  the  derivatives  of  the 
state  approximations  between  collocation  points.  If  this  is  not  the  case,  the  largest 
value  in  each  row  is  collected  into  a  vector,  representing  the  greatest  error  with  respect 
to  the  dynamics  for  each  segment.  The  arithmetic  mean  of  these  maximum  errors  is 
taken,  and  the  errors  are  scaled  by  the  arithmetic  mean.  This  allows  easy  comparison 
of  the  errors.  For  the  case  where  one  error  is  significantly  higher  than  the  rest,  a 
problem  at  a  specific  time  is  assumed,  most  likely  a  result  of  a  discontinuity.  The 
number  of  segments  is  therefore  increased  and  another  iteration  is  performed,  with  a 
segment  break  at  the  problematic  time  to  increase  nodal  density  there.  Uniform-type 
errors  exist  when  all  error  values  are  relatively  equal.  If  this  is  below  tolerances, 
the  solution  is  complete.  If  not,  a  poor  approximation  is  assumed  and  the  total 
number  of  nodes  is  increased  for  the  next  iteration,  resulting  in  a  higher  order  state 
approximation. 
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VIII.  Quadrotor  Vehicle  Description  and  Flight  Control 

Development 


“You  go  to  war  with  the  army  you  have,  not  the  army  you  want” 

— Former  Secretary  of  Defense  Donald  H.  Rumsfeld 


erification  of  the  effectiveness  of  the  RTOC  algorithm  beyond  simula¬ 
tion  was  performed  with  an  in-house,  custom  built  quadrotor  helicopter 


(Figure  38),  designed  at  the  Air  Force  Institute  of  Technology’s  (AFIT)  Advanced 
Navigation  Technology  (ANT)  Center.  The  flight  control  system  for  the  aircraft  was 


Figure  38.  Quadrotor  Helicopter 


designed  with  a  much  simpler  purpose  in  mind,  and  significant  modifications  had  to 
be  made  in  order  to  make  the  power  line  landing  possible.  This  chapter  details  the 
description  of  the  vehicle,  as  well  as  some  of  the  flight  control  challenges  and  solutions 
used  for  the  flight  test. 
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8.1  Vehicle  Description 


The  quadrotor  consisted  of  a  0.607-m  square  frame  with  four  22.86-cm  blades 
driven  by  Goldline  AXI  2212/20  brushless  motors.  The  motors  were  regulated  with 
Phoenix  25  speed  controllers  and  powered  by  two  Li-Polymer  2200  mAh,  11.  IV,  3-ccll 
batteries.  A  Pico-ITX  (Linux  Ubuntu)  with  a  VIA  C7  1-GHz  processor  with  1-GB 
of  RAM  on  top  of  the  aircraft  was  used  for  data  collection  and  processing  of  images 
from  a  Logitech  Quickcam  Pro  9000  webcam.  As  the  line  detection  algorithm  was  not 
complete,  the  bearing  measurements  for  the  flight  test  were  provided  by  the  Vicon 
system  and  corrupted  by  noise,  vice  using  the  camera.  Accelerations  were  measured 
with  an  Analog  Devices  ADIS  16355AMLZ  MEMS-IMU,  and  inner-loop  flight  control 
processing  was  performed  on  a  custom  PIC-24  microcontroller  circuit  board.  Outer- 
loop  RTOC  guidance  was  provided  by  an  algorithm  running  in  Matlab®  R2009a 
(Microsoft  XP),  passed  to  a  ground  station  (Linux  Ubuntu)  via  a  dealer  function. 
Mid-loop  control  commands  were  generated  within  the  ground  station  custom  C  code 
using  a  GTK  graphics  package,  and  communication  to  the  vehicle  was  across  a  2.4- 
GHz  XBee  Pro  serial  modem.  Both  computers  were  Dell  360  2.0-GHz  laptops  with 
2-GB  of  RAM.  Position  feedback  and  flight  test  data  was  provided  with  a  Vicon 
Tracker  motion  capture  system  using  60  near  IR  (~750-nm)  cameras.  A  schematic 


of  the  overall  system  is  shown  in  Figure  39 


Thrust  for  the  quadrotor  is  supplied  by  four  independently  controlled,  fixed-pitch 
propellers.  The  propellers  in  opposing  corners  spin  the  same  direction,  as  shown  in 


Figure  40  Altitude  is  controlled  by  varying  the  thrust  from  all  four  motors  simulta¬ 
neously.  Pitch  and  roll  are  controlled  by  increasing  the  thrust  of  both  motors  on  one 
side  of  the  applicable  axis,  and  decreasing  the  thrust  on  the  other  side.  The  total 
thrust  remains  near  constant,  maintaining  altitude  at  small  angles.  Since  both  sides 
have  one  propeller  turning  clockwise  and  one  turning  counter-clockwise,  the  total 
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Figure  39.  Quadrotor  System  Schematic 

torque  also  remains  the  same,  maintaining  heading.  Heading  is  controlled  with  an 
increase  and  decrease  of  opposing  pairs,  maintaining  total  thrust  while  changing  the 
total  torque. 


Figure  40.  Quadrotor  Opposing  Pitch  Propellers 


8.1.1  Autopilot  Overview. 

Based  on  the  RTOC  structure  developed  in  Chapter  |VH  the  autopilot  architecture 
was  designed  with  three  main  loops.  The  inner  stabilization  loop  produces  the  actual 
Pulse  Width  Modulation  (PWM)  signals  that  drive  the  motors.  Inputs  are  the  body 
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axis  angular  rate  measurements  from  the  on-board  IMU,  approximations  of  angular 
accelerations  based  on  a  discrete,  first-order  lag  model,  and  error  commands  in  the 
appropriate  channels  from  the  portion  of  the  controller  in  the  ground  station.  The 
specific  structure,  gain  placements  and  values,  vehicle  moments  of  inertia,  and  such 
can  be  found  in  the  Simulink  diagrams  and  initialization  file  in  Appendix  [A]  but 


a  simplified  control  flow  diagram  is  shown  in  Figure  41  The  inner  feedback  loop 
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Figure  41.  Primary  Autopilot  Loops 


regulates  the  angular  rates  and  accelerations  to  zero,  while  accepting  the  autopilot 
commands  of  the  mid-loop,  which  compares  the  current  position  and  heading  with 
state  vector  that  is  commanded  at  that  time  from  the  most  current  trajectory  time 
history  of  the  path  planner.  The  path  planner  takes  the  measurements  from  the 
bearing  sensor  and  plans  a  new  optimal  path,  using  the  last  optimal  solution  as  an 
initial  guess. 


8.2  Flight  Control  Modifications 

The  quadrotor  flight  control  system  was  originally  designed  to  hover  at  a  point. 
The  point  could  be  moved  with  hand-controlled  inputs.  Actual  steady-state  tracking 
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of  that  point  was  extremely  poor,  but  immaterial,  as  the  aircraft  was  flown  visually. 
If  the  vehicle  was  low,  the  commanded  point  was  moved  up — how  close  the  vehicle 
actually  was  to  the  commanded  hover  point  was  unknown. 


Several  flight  control  modifications  were  required  to  enable  automated  path  control 
of  the  aircraft  and  the  ability  to  fly  to  an  exact  point  with  no  steady-state  offset. 
Some  suboptimal  decisions  had  to  be  made  that  led  to  a  design  that  was  functional, 
but  incomplete.  The  actual  quadrotor  used  was  the  “spare,”  as  the  primary  aircraft 
suffered  a  catastrophic  crash  just  prior  to  commencing  this  research  due  to  an  error 
in  a  line  of  code.  With  a  fragile,  naturally  unstable  aircraft  and  no  spare  parts,  a 
minimalist  approach  was  taken  to  control  development,  changing  the  original  design 
and  code  as  little  as  possible.  The  decision  to  limit  the  desired  flight  control  work  was 
validated  somewhat  by  an  irreplaceable  IMU  on  the  custom  servo-sensor  board  failing 
several  times  prior  to  takeoff  (luckily)  during  testing,  and  eventually  burning  out  the 


entire  board  a  few  sorties  after  the  last  of  the  flight  tests  presented  in  Chapter  IX 


Due  to  the  necessary  caution,  the  ideal  course  of  changing  the  control  scheme  to  feed¬ 
forward  control  based  on  the  optimal  solution  was  not  attempted,  choosing  instead  to 
simply  schedule  the  motion  of  the  hover  point  in  accordance  with  the  optimal  path. 
Clearly,  this  will  result  in  late  turns  and  overshoots  during  more  aggressive  maneuvers 
as  the  commands  to  turn  are  not  applied  until  an  error  already  exists  between  the 
vehicle  and  the  path.  This  is  most  noticeable  in  the  horizontal  channels,  as  the  control 
of  the  engines  does  not  directly  apply  force  in  that  axis,  but  must  first  generate  and 
integrate  angular  rate. 


8. 2. 1  Simulation. 

All  flight  control  work  was  developed  in  simulation  to  minimize  risk.  Without 
an  aircraft  model  or  any  documentation  of  the  flight  controls,  the  Simulink  model  in 
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Appendix  [A]  was  created  by  backing  out  the  ground  station  C  code  and  the  code  from 
the  servo-sensor  board  on  the  vehicle,  and  applying  kinematic  and  dynamic  equations 
from  first  principles  as  detailed  in  [83]  •  These  equations  were  modified  slightly  based 
on  comparison  of  expected  flight  profiles  to  flight  test  data,  adding  a  drag  term  in 
each  axis.  The  drag  component  was  more  pronounced  in  the  vertical  axis  at  the  slow 
speeds  of  the  quadrotor,  reflecting  both  propeller  drag  and  a  heaving  derivative  effect 
common  in  helicopter  models.  The  heaving  derivative  reflects  the  fact  that  as  vertical 
velocity  increases,  the  angle  of  attack  on  the  blades  decreases,  resulting  in  less  lift. 
The  resulting  moment  equations  took  body  axis  moments,  [L  M  N]T ,  which  were 
known  from  the  engine  model  and  respective  locations  of  each  motor,  and  integrated 
them  to  find  the  body  axis  angular  velocities,  \p  q  r]T .  The  equations  were  simplified 
for  the  quadrotor,  which  can  be  considered  symmetric  in  both  the  Xj,Zb  and  the  y^Zi, 
planes: 


P  =  lL  -  Qr  {h*  -  Jro)] 

J-xx 

(154) 

q=  ~j—[M  -rp  (Ixx  -  Izz)] 

*yy 

(155) 

^  ~7  PQ  (lyy 

1-zz 

(156) 

The  body  axis  angular  velocities  were  then  integrated  to  find  the  Euler  angles: 

0  =  q  cos  (j>  —  r  sin  (p 

(157) 

(p  —  p  +  q  sin  0  tan  6  +  r  cos  0  tan  6 

(158) 

ifj  —  (q  sin  0  +  rcosp )  sec  6 

(159) 

With  the  only  forces  being  the  thrust  from  the  propellers,  Fz,  and  a  first-order 
drag  force  approximation,  the  force  equations  were  integrated  to  find  the  body  frame 
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velocities  [u  v  w]T: 


ii  =  tv  —  qw  —  g  sin  6  —  kr,xu  (160) 

v  =  pw  —  ru  +  g  cos  9  sirup  —  kf)yv  (161) 

w  =  qu  —  pv  +  g  cos  Ocoscj)  +  Fz/m  —  k^zw  (162) 


where  m  is  the  mass  and  g  the  gravitational  constant.  With  the  body  frame  velocities 
and  Enler  angles,  the  final  simulator  step  is  to  rotate  to  the  navigation  frame  and 
integrate  for  position: 


x 
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CtpSgCip  +  S<pS^j 
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(163) 


where  =  cos  etc. 

With  the  simulator  established,  the  flight  control  structure  was  added.  A  few  of 
the  interesting  features  are  summarized  below.  For  full  detail  on  the  flight  controls 
and  specifics  such  as  gains  and  moments  of  inertia,  see  Appendix  [A] 


8.2.2  Vertical  Control  Channel. 

The  system  was  initially  controlled  with  a  proportional-derivative  (PD)  scheme, 
using  a  nominal  throttle  trim  setting  to  offset  the  weight,  and  adjusting  all  four  en¬ 
gines  around  this  setting  to  correct  vertical  position  error.  Though  effective  for  hand 
flying,  significant  steady-state  error  existed  between  the  commanded  and  actual  ver¬ 
tical  positions,  as  show  in  Figure  [42]  The  nominal  throttle  setting  was  clearly  too  low 
to  account  for  the  weight  of  the  vehicle.  To  avoid  unnecessary  tuning  flights,  a  force 
test  stand  was  built  to  model  the  non-linear  relationship  between  thrust  and  PWM 
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Figure  42.  Need  for  Vertical  Error  Integration 

command  to  more  accurately  predict  the  motor  performance.  This  was  helpful,  but 
still  insufficient  for  a  precision  landing  system  without  active  error  integration,  as  the 
true  nominal  throttle  trim  will  vary  based  on  loss  of  battery  strength.  Furthermore, 
if  the  nominal  throttle  trim  is  set  correctly  for  flight,  the  aircraft  will  “leap”  during 
takeoff,  when  ground  effect  makes  the  propellers  much  more  efficient. 

The  nominal  thrust  was  set  low  to  match  the  performance  in  ground  effect  for 
a  good  takeoff,  and  a  discrete  error  integrator  was  added  to  correct  it  during  flight. 
Integration  is  by  nature  destabilizing,  so  a  very  conservative  gain  level  was  selected 
from  a  root  locus  plot  of  the  Simulink  model  linearized  about  a  hover  condition.  For 
the  full  land-on- a-wire  test  flights,  the  aircraft  was  flown  to  a  specific  hover  position 
before  the  run,  which  started  at  the  30  second  point,  so  there  was  ample  time  to  find 
the  correct  nominal  trim.  With  additional  test  sorties,  this  could  be  improved. 

To  avoid  integrator  windup  prior  to  takeoff  while  the  ground  station  controller 
is  running,  “reset”  logic  was  added  to  re-zero  the  integrated  error  value  when  the 
motors  were  not  engaged.  As  will  be  discussed  in  the  horizontal  channel,  saturation 
limits  were  applied  on  both  the  integrated  error  value  and  the  amount  of  proportional 
vertical  error  visible  to  the  system  in  order  to  limit  the  maximum  vertical  speed. 


8.2.3  Horizontal  Control  Channels. 


The  horizontal  channels  are  controlled  by  differential  power  to  produce  either  pitch 
or  bank  rate.  Position  errors  in  the  navigation  frame  are  rotated  by  heading  into  the 
body  frame  with  a  simple  direction  cosine  matrix,  with  the  assumption  that  bank 
and  pitch  angles  are  small: 
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(164) 


Velocity  in  the  navigation  frame  is  calculated  considering  the  position  change  in 
a  single  Vicon  capture  frame  over  the  frame  interval,  likewise  rotated  into  the  body 
axis.  Originally,  PD  control  on  position  error  was  applied  with  angle  feedback  of  <f>  or 
6,  respectively.  This  resulted  in  acceptable  performance  for  hand  flying,  but  in  terms 
of  inertial  precision,  the  aircraft  was  ±0.5-m  from  the  commanded  hover  position  at 
any  point  in  time  (for  comparison,  it  is  desired  that  the  vehicle  be  within  ±0.038-m  of 
commanded  position  to  pass  through  the  vertical  “mouth”  of  the  hook,  assuming  zero 
uncertainty  in  the  estimated  position  of  the  wire).  Integral  control  with  saturation 
and  reset  logic  were  added,  and  gains  were  tuned  for  performance. 

The  original  design  did  not  have  a  hard  limit  bank  or  pitch  angle,  as  it  was 
designed  to  be  hovered  in  a  small  flight  area  with  a  hand  controller  that  could  only 
make  small  changes  in  commanded  position.  With  a  large  flight  area  and  the  prospect 
of  a  (newly  designed)  automated  command  system,  the  potential  existed  to  command 
a  large  change  in  position,  which  could  flip  the  vehicle  over.  Saturation  limits  on 
the  amount  of  position  error  entering  the  command  channel  were  added.  Because 
each  channel  was  dampened  with  velocity  feedback,  the  desire  to  move  the  aircraft 
to  a  point  was  counter-balanced  by  the  desire  to  keep  velocity  at  zero.  Saturating 
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the  position  error  input  effectively  set  a  maximum  velocity  limit.  With  the  system 
balanced  at  steady-state,  the  desired  maximum  velocity  value  for  both  axes  could  be 
adjusted  for  the  velocity  and  proportional  feedback  gains  to  determine  the  required 
level  of  saturation  (the  level  of  saturation  required  to  produce  the  right  maximum 


velocity — Figure  67  on  page  178  may  make  this  more  clear): 


X err sat  ~  Vmaxx  *  kVx/k 


Vx 


(165) 


Prescribing  a  maximum  horizontal  velocity  value  indirectly  limits  the  bank  and 
pitch  angles.  The  quadrotor  needs  only  a  small  horizontal  thrust  component  to  begin 
moving  from  a  hover,  as  it  is  delicately  balanced.  Once  moving  to  a  steady-state 
velocity,  the  bank  is  removed  as  the  aircraft  “coasts,”  much  like  a  puck  on  an  air 
hockey  table.  At  the  low  speeds  used  for  this  research,  a  very  minor  amount  of  bank 
or  pitch  (approximately  10°)  was  required  to  reach  steady-state  velocity,  and  only  a 
negligible  amount  is  required  to  overcome  drag  and  sustain  it,  as  can  be  seen  in  the 
step  commands  of  Figure  [43]  With  bank  and  pitch  angles  limited  by  the  position 
error  saturation,  the  aircraft  cannot  flip  over,  regardless  of  the  size  of  an  erroneous 
input.  As  an  additional  benefit,  the  low  pitch  and  bank  requirements  work  out  well 
for  a  vehicle  with  a  fixed  camera  that  needs  to  keep  the  target  in  the  field  of  view. 

Another  feature  installed  was  a  variant  of  anti-windup  tracking  dubbed  “sneak- 
back”  logic.  Essentially,  when  the  aircraft  is  flying  to  a  point  beyond  the  position 
error  saturation  distance,  the  integrator  will  saturate  in  the  same  direction.  When 
the  aircraft  reaches  the  point,  a  large  overshoot  will  occur.  To  reduce  this  effect,  the 
integrator  is  smoothly  pulled  back  toward  zero  whenever  the  position  error  is  on  or 
near  its  limit.  With  this  implementation,  a  design  decision  must  be  made  with  regard 
to  the  maximum  velocity  limit.  If  the  gain  balance  to  achieve  the  correct  steady-state 
velocity  is  set  considering  a  saturated  integrator  input,  then  the  steady-state  veloc- 
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Figure  43.  Maximum  Velocity  Step  Commands 

ity  will  be  below  the  maximum  as  the  integrator  gets  pulled  back  to  zero.  If  the 
gain  balance  is  set  considering  the  integrator  output  to  be  zero,  the  vehicle  will  have 
the  desired  maximum  velocity  at  steady-state,  but  may  overspeed  briefly  while  the 
integrator  settles.  The  latter  option  was  considered  acceptable  and  implemented. 

8.2.4  Heading  Control  Channel. 

The  least  amount  of  work  was  accomplished  in  the  heading  channel,  and  it  is  the 
least  well  controlled.  Figure  |44| shows  representative  heading  error  on  an  early  flight. 
Error  integration  was  necessary  and  added,  with  the  associated  reset  logic  and  satura¬ 
tion.  Due  to  the  natural  damping  when  controlling  with  torque,  derivative  action  was 
not  required,  resulting  in  a  proportional-integral  (PI)  channel.  The  feedback  gains 
were  increased  based  on  gain  margin  in  the  linearized  Simulink  model  and  fine  tuned 
in  flight  test.  A  clear  difficulty,  in  all  channels,  is  the  fact  that  control  is  modeled  as 
decoupled,  but  isn’t  in  reality — especially  with  respect  to  errors,  which  are  typically 
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Figure  44.  Poor  Heading  Control 


not  aligned  with  an  axis.  Bending  of  the  aircraft  body,  imbalanced  power  output, 
and  misaligned  propellers  contribute  to  the  steady-state  errors  that  are  seen  in  Fig¬ 


ure 


44  Integration  can  balance  out  the  errors  in  a  hover,  but  it’s  just  that — perfectly 


balanced,  and  susceptible  to  the  slightest  disturbance.  Every  time  the  power  settings 
are  altered  for  any  maneuver  or  change  of  orientation,  the  error  balance  is  changed. 


8.2.5  Automated  Flight. 

In  order  be  able  to  command  the  system  automatically,  an  automatic  flight  mode 
was  added  that  accepted  command  inputs  from  the  dealer  function,  shown  in  Fig¬ 


ure 


39 ,  as  if  they  had  come  from  the  hand  controller.  The  dealer  received  the  optimal 


path  time  history  from  the  path  planner,  and  determined  the  current  location  in  the 
series.  Line  numbers  with  a  constant  update  rate  were  used  vice  true  time,  to  avoid 
clock  synchronization  errors  between  the  computers.  The  correct  commands  for  po¬ 
sition  and  heading  were  then  sent  to  the  ground  station,  while  the  measurements  of 
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the  wire  position  and  the  vehicle’s  current  measured  location  were  returned  to  the 
path  planner. 

Figures [45] and [46] show  one  of  the  early  paths,  flying  in  circles  with  varying  altitude 
followed  by  level  circles  to  test  the  vehicle’s  ability  to  follow  a  constantly  arcing  path. 
At  this  point,  the  tracking  had  been  markedly  improved,  but  the  heading  channel 
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Figure  45.  Automatic  Flight  Control  Development  Test 

appeared  to  have  had  much  more  difficultly  with  the  level  circles  than  the  slanted 
ones,  though  the  reason  is  unclear.  The  larger  heading  errors  actually  begin  18  seconds 
prior  to  the  level-off,  so  it  may  not  be  altitude  related. 

For  safety,  a  “hover”  mode  was  added  that  allows  an  observer  pilot  to  switch  back 
to  hand  controller  commands,  with  a  second  actuation  zeroing  out  the  integrators,  in 
case  they  were  the  cause  of  the  needed  takeover.  At  the  moment  the  hover  mode  was 
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(a)  Varying  Altitude,  t= 25  sec  to  t= 88  sec  (b)  Level  Circles,  t= 88  sec  to  t=154  sec 


Figure  46.  Flight  Path  for  Automatic  Flight  Control  Development  Test 

activated,  the  hand  controller  commands  were  zeroed  out,  and  the  current  position 
and  orientation  were  captured.  The  captured  positions  were  then  continually  added 
as  an  offset  to  the  commands  from  the  hand  controller,  resulting  in  a  hover  that  could 
be  landed  manually. 


8.2.6  System  Identification. 


The  derived  simulator  had  far  less  damping  than  the  real  aircraft,  making  use  of 
the  simulator  for  gain  selection  of  little  value.  A  data  capture  algorithm  was  written 
and  installed  on  the  quadrotor,  allowing  flight  data  to  be  run  through  the  simulation 


for  comparison  of  expected  and  actual  performance.  Figure  [47]  shows  a  safety  flight 
flown  in  to  a  simulated  wire  position  compared  to  the  uncompensated  simulation 
output  for  the  same  commanded  path. 

System  identification  techniques  were  used  to  add  a  compensator  for  the  errors, 
and  the  aforementioned  drag  terms  were  added.  In  hindsight,  the  model  deficiencies 
are  most  likely  due  to  the  fact  that  the  dampening  torque  effect  from  the  spinning 
propellers  was  not  accounted  for.  Additional  terms  should  have  been  added  for  this 


in  Equations  157  159  As  performed,  however,  the  compensated  system  did  a  much 
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Figure  47.  Simulator  without  Modification  to  Match  Flight  Test  Data 


better  job  at  matching  the  true  system,  as  shown  in  Figure  48  This  made  both  gain 
selection  and  design  of  the  flare  mode  much  more  effective. 


8.2.6. 1  Flight  in  the  fiAVIARI. 

Full-scale  flight  tests  were  accomplished  in  the  Micro  Air  Vehicle  Integration  and 
Application  Research  Institute  (/rAVIARI)  indoor  flight  test  facility.  Several  consid¬ 
erations  had  to  be  addressed  to  enable  flight  in  new  facility,  most  of  which  had  to  do 
with  networking  with  different  computers.  The  Vicon  software  was  also  different  be¬ 
tween  the  ANT  Center  and  the  /iAVIARI,  but  the  actual  data  stream  is  consistent,  so 
only  minor  software  changes  were  required,  along  with  DCM  changes  for  the  different 
reference  frames.  The  hand  controller  code  also  had  to  be  modified,  as  the  original 


145 


Figure  48.  Simulator  Modified  to  Match  Flight  Test  Data 


method  used  each  press  of  a  button  to  move  the  hover  point  a  percentage  of  the  flight 
arena  size  (the  trim  accumulators  on  the  hand  controller  only  moved  between  -1  and 
1).  As  the  arena  got  10  times  larger,  the  hand  controller  became  10  times  as  sensitive. 
Logic  was  added  with  a  transformation  to  scale  each  button  actuation  to  an  actual 
distance. 

Lastly,  a  hook  was  fashioned  from  welding  rod  as  a  simple  method  of  attaching 
the  quadrotor  to  the  wire.  The  very  flexible  nature  of  the  hook  in  concert  with  the 
vibration  of  the  quadrotor  excited  a  large  harmonic  oscillation.  Several  iterations  of 
dampening  lines  were  added  to  the  hook  until  the  flight  characteristics  were  satisfac¬ 


tory.  These  lines  can  be  seen  in  Figure  16  on  page  53 
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IX.  Results  and  Analysis 


“In  science,  you  can  lie  and  fudge  the  data  because  you  don’t  have  to  make 
anything  work.  In  engineering,  the  product  is  the  proof  of  your  honesty.” 

— Pepper  White 


✓  m  he  functionality  of  the  RTOC  system  was  validated  through  extensive  simula- 
tion  during  development,  and  verification  of  the  algorithm’s  ability  to  accom¬ 
plish  the  mission  of  landing  on  a  wire  was  accomplished  through  flight  test.  The  flight 
test  profiles  were  performed  in  the  /rAVIARI,  operated  by  the  Air  Vehicles  Directorate 


of  the  Air  Force  Research  Laboratory  (AFRL/RB),  and  shown  in  Figure  49  The  in- 


Figure  49.  AFRL/RB  /iAVIARI  Indoor  Flight  Test  Facility 


door  flight  test  lab  allowed  use  of  the  Vicon  camera  system,  a  flight  requirement  of 
the  available  research  vehicle.  Though  the  /iAVIARI  is  very  large  for  an  indoor  flight 
test  lab,  the  true  power  line  landing  scenario  is  larger,  and  the  geometry  was  scaled 
to  fit  within  physical  limitations.  An  average  medium-voltage  (distribution)  utility 
pole  is  approximately  10-m  high,  but  the  safe  maximum  height  to  maintain  visibility 
by  a  sufficient  number  of  Vicon  tracking  cameras  in  the  indoor  flight  facility  was 
5.5-m.  The  walls  of  the  facility  dictated  a  maximum  range  of  approximately  18-m, 
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well  inside  of  the  expected  range  at  which  a  power  line  could  be  confidently  identified 
with  a  webcam  type  sensor.  Correspondingly,  the  flight  test  was  scaled  down  in  size 
to  fit  the  /iAVIARI,  and  the  vehicle  speed  was  reduced  to  produce  a  likely  approach 
segment  time  of  about  30  seconds. 


9.1  Simulation  Results 

In  order  to  test  the  robustness  of  the  system  and  the  reliability  of  both  the  es¬ 
timation  and  optimization  algorithms,  a  Monte  Carlo-style  simulation  of  1000  runs 
was  performed  on  the  same  scale  as  the  flight  tests  to  maintain  comparability.  The 
run  number  was  pre-selected,  and  the  resulting  solution  parameters  of  average  loop 
time,  mean  error,  and  final  directional  covariance  were  confirmed  to  have  converged 
to  within  10~3  of  their  respective  units.  The  problem  geometry  was  varied  by  moving 
the  actual  target  location  from  the  initial  estimate: 


xt~jV(xto,4:9  m2) 

(166) 

zt~Af(zto,4:  m2) 

(167) 

Outliers  were  limited  to  stay  within  the  allowable  flight  space  vertically  and  high 
enough  to  maintain  the  approach  point  above  the  allowable  floor.  The  initial  pickup 
range  was  also  limited  to  a  minimum  of  12-m  to  provide  some  room  to  maneuver 
(without  some  limit,  the  power  line  may  unrealistically  initialize  behind  the  vehicle). 
Real-world  considerations  must  include  a  contingency  plan  for  a  “go-around”  for 
exceptionally  late  or  missed  sensor  pickups.  The  difference  in  vertical  and  horizontal 
certainty  reflects  the  fact  that  more  knowledge  will  exist  concerning  the  height  of  the 
power  line  than  of  the  initial  sensor  pickup  range.  For  a  full-scale  system,  actual 
sensor  capability,  engineering  judgment  about  likely  power  line  height  variance,  and 
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the  amount  of  confidence  in  the  mapped  power  line  locations  should  be  included  in 
the  selection  of  the  initial  covariance,  Po.  Disturbances  from  effects  such  as  wind 
gusts  were  added  with  a  bivariate  Gaussian  distribution,  adding  a  random  variance 
in  the  vehicle  location  sampled  at  the  time  of  each  measurement,  and  measured  by 
the  own-ship  navigation  system: 


[  %meas 


Zmeas 


]T  ~  A/*2 


X 


(168) 


The  simulation  was  initiated  with  the  conditions  found  in  Table  [2]  The  shape 
of  each  instantaneous  optimal  trajectory  varies  based  on  the  information  available 


to  the  system  at  the  time.  Figure  [50]  shows  typical  solutions,  with  specific  problem 
parameters  varied  to  highlight  key  features.  The  results  show  complete  trajectories 
for  the  remainder  of  the  flight,  as  are  provided  at  every  epoch  by  the  path  planner. 
The  characteristics  shown  are  helpful  in  creation  of  heuristics  to  mimic  the  optimal 
solution,  potentially  a  requirement  for  sUASs  without  the  processing  capacity  for 
RTOC.  All  maneuvering  in  the  simulation  is  restricted  to  the  vertical  plane.  Gener¬ 
ally,  the  length  of  the  run  (note  the  asymmetric  axes  lengths)  allows  a  greater  amount 
of  information  to  be  collected  about  the  vertical  position  of  the  target,  requiring  the 
trajectory  planner  to  move  away  from  the  initial  LOS  angle. 


Table  2.  Simulation  Limitations  and  Initial  Parameters 


Parameter 

Value 

Parameter 

Value 

Est.  Loop  Time 

0.9  s 

Prr _ ,  P zz _  0.02 

m2 

ft  min  5  ft  max 

O 

O 

o 

O 

CO 

1 

App.  Offset 

(m) 

-2 

0.4 

]T 

0.071  rad2 

xt(m) 

9 

*]T 

\vA  ,  \v ?  1 

1  x  1  max  ’  1  z\  max 

0.5  m/s 

x0 

-8 

3  0  0  p-,1,  p-‘  0] 

\uT\  ,  \uJ 

1  x  1  max  ’  1  z\  max 

0.5  m/s2 

Po(m2) 

80 

0 

0 

80 

^min  i  %max 

0.8,  6  m 
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Lower  FOV  Limit 


x  ( m ) 


x  (m) 


(a)  Typical  Solution  Shape 


(b)  Speed  and  Accel  Constraints  only 


(d)  Relaxed  Final  Covariance  Requirements 

(Pzxma!B  =  Pzzma:D  =  0.2  m2) 


Figure  50.  Instantaneous  Trajectory  Shape  Sensitivity  to  Constraints 


Figure  [o0|i  shows  the  characteristic  shape  for  the  typical  initial  conditions,  where 
the  system  is  directed  to  climb  to  the  maximum  allowable  altitude,  or  ceiling,  for  a 
“high  look,”  moving  to  obtain  a  “low  look”  at  the  end-game  where  the  measurements 
are  more  effective  due  to  the  close  range.  This  path  visually  increases  the  information 


elements  seen  in  Equation  73  on  page  67  FOV  limitations  keep  the  vehicle  in  a 
position  where  the  target  will  be  visible  to  the  fixed  camera,  and  a  “safe  approach” 
line  is  enforced  to  keep  the  aircraft  from  flying  past  the  desired  approach  point  and 
backing  up.  Though  certainly  within  the  capabilities  of  the  quadrotor,  it  was  deemed 
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unsafe  to  intentionally  proceed  inside  the  approach  point  until  the  certainty  in  the 
actual  power  line  position  was  within  the  required  limits. 


The  second  panel,  Figure  50  3,  shows  an  instantaneous  flight  trajectory  with  the 
allowable  flight  envelope  limitations  removed,  maintaining  the  speed  and  acceleration 
limits  in  the  dynamic  constraints.  The  final  required  target  position  certainty  was 
increased,  to  highlight  the  fact  that  the  optimal  solution  may  intentionally  include 
transients  inside  of  the  safe  approach  line  if  not  enforced.  For  the  submarine  formu¬ 
lation  of  the  problem,  this  trajectory  shape  represents  an  optimal  horizontal  solution 
that  would  be  encountered  on  a  larger  scale,  with  the  exception  of  the  last  transient, 
which  would  be  avoided  by  implementing  a  circular  path  constraint  to  stay  beyond 
the  opponent’s  maximum  torpedo  range  until  the  target  position  is  resolved.  All  of 
the  characteristics  of  the  vehicle  are  simply  parameters  that  can  be  set  as  appropriate 


for  an  individual  system.  The  third  panel,  Figure  50  z,  shows  the  path  solution  for 
a  camera  with  a  more  narrow  FOV  (30°).  The  most  extreme  target  approach  angle 
is  held  as  long  as  necessary.  For  times  when  the  final  certainty  requirements  do  not 
differ  greatly  from  the  current  covariance  estimate,  only  a  small  excursion  is  necessary 
to  gain  the  needed  amount  of  information,  as  shown  in  Figure  [50jl. 

Obviously,  every  active  parameter  in  the  optimal  control  problem  contributes  to 
the  final  shape  of  the  trajectory,  but  the  sensitivity  of  a  few  dominant  parameters 
found  during  the  research  was  considered  noteworthy.  The  immediate  move  away 
from  the  initial  LOS  angle  is  predictable.  The  “hook”  at  the  end  of  the  path  shown 
in  Figures  [50|i-|o0}:  is  also  dominant,  taking  advantage  of  the  wide  angular  spectrum 
at  close  range  for  the  greatest  increase  in  information.  For  a  workable  heuristic,  the 
initial  move  away  from  the  first  LOS  angle  provides  the  range  observability  necessary 
to  determine  when  to  make  the  “hook,”  which  could  be  initiated  at  the  6-m  remaining 
point  at  this  speed.  Though  the  characteristic  shape  is  the  same,  the  range  at  which 
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the  “hook  point”  is  executed  is  non-linear  and  not  necessarily  directly  scalable  to  a 
larger/faster  problem.  To  find  it  for  a  particular  system,  the  algorithm  should  be  run 
in  simulation  with  system  specific  limitations  and  expected  conditions. 


9.1.1  Local  Minima. 


The  geometry  of  the  problem  creates  a  bimodal  solution  space.  With  {30  =  0, 
symmetric  boundary  constraints,  no  initial  vertical  velocity,  and  the  approach  point 
level  with  the  target,  an  optimal  path  that  initially  moved  up  would  have  a  mirrored 
path  with  an  initial  move  down  and  the  same  total  cost.  Global  favorability  of  a  “high 
road”  versus  a  “low  road”  local  minimum  is  dependent  on  the  initial  state  when  the 
first  measurement  becomes  available.  For  heuristics,  the  overall  direction  tends  to  be 
high  if  the  initial  position  of  the  vehicle  is  low  relative  to  the  target,  and  vice  versa. 
Stronger  factors  are  initial  vertical  velocity  (tends  to  continue  in  the  initial  direction), 
and  the  vertical  difference  between  the  approach  point  and  the  actual  height  of  the 
target  (if  the  approach  point  is  low,  the  initial  move  is  typically  high  and  vice  versa — 


note  the  final  approach  point  difference  between  Figure  5Cb  and  Figure  50  i).  The 


amount  of  maneuvering  room  between  the  altitude  limits  and  the  estimated  target 
position  estimate  also  impacts  this  decision. 

For  this  system,  experience  has  shown  that  the  “high  road”  is  the  global  minimum 
for  the  given  target  height,  with  the  greatest  sensitivity  being  to  the  approach  point 
being  set  below  the  target  height  to  account  for  the  height  of  the  hook  above  the  ve¬ 
hicle.  A  biased  guess  is  not  necessary  to  find  the  global  minimum,  and  only  the  initial 
and  final  points  are  used  to  initialize  the  system.  As  a  practical  method  for  a  system 
with  less  certain  characteristics,  the  global  minimum  for  the  initial  target  position 
guess  can  be  found  by  checking  both  initial  directions  a  priori  through  simulation 
with  the  planned  initial  target  estimate  and  covariance,  using  initial  path  guesses  bi- 
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ased  in  each  direction  (the  algorithm  can,  at  times,  be  fooled  into  a  local  minimum  in 
this  manner).  The  global  solution  found  should  be  the  seed  for  the  initial  calculation 
of  the  real-time  path  planner,  which  will  actually  begin  to  fly  this  solution  between 
the  time  the  first  measurement  is  received  and  the  time  the  first  optimal  trajectory 
is  produced,  which  will  have  been  solved  for  using  the  initial  velocity  in  the  correct 
direction  expected  at  time  t2o.  Once  the  initial  trajectory  has  been  begun,  switching 
to  the  other  minimum  becomes  costly  clue  to  the  control  required  to  overcome  the 
initial  vertical  velocity,  and  the  increased  percentage  of  the  path  that  is  left  near  the 
“middle”  of  the  flight  envelope,  near  the  target  altitude.  Flight  in  this  area  con¬ 
tributes  little  information  about  the  range  to  the  target,  which  is  the  “long  pole  in 
the  tent”  in  terms  of  the  optimization. 

For  the  second  flight  test,  the  initial  target  estimate  was  intentionally  fabricated 
to  make  the  “low  road”  the  initial  global  minimum.  This  was  done  by  intentionally 
setting  the  initial  guess  too  close  to  the  ceiling  limit  to  allow  the  vehicle  room  to  ma¬ 
neuver  above  it.  The  “low  road”  scenario  was  demonstrated  because  it  could  possibly 
be  encountered  with  a  significantly  erroneous  target  position  estimate,  though  this  is 
unlikely.  Note  that  in  the  lab,  the  initial  relative  position  of  the  power  line  was  fixed 
physically,  and  the  initial  target  position  estimate  was  varied  to  cause  differences  in 
path  selection.  The  simulations  were  set  up  to  mimic  this,  producing  several  unlikely, 
but  possible,  scenarios  where  the  target  was  very  near  the  floor,  or  in  the  upper 
“corner”  of  the  flight  envelope,  such  as  in  Flight  Test  #2.  This  was  a  good  test  of 
robustness  to  potentially  poor  target  estimates,  but  in  a  true  sUAS  landing  scenario, 
the  initial  relative  target  estimate  will  be  fixed  (based  on  the  expected  parameters  of 
the  sUAS  at  initial  sensor  pickup).  The  initial  solution  will  therefore  be  the  same  for 
every  run  (both  initial  directions  having  been  checked  a  priori ),  and  the  vehicle  will 
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have  already  committed  to  the  global  minimum  direction  during  the  first  calculation 
epoch. 


9.1.2  Timing  and  Accuracy. 


For  the  1000  simulation  runs,  the  average  loop  time,  including  optimization  cal¬ 
culation,  communication,  UKF  calculations,  and  all  delays  was  0.82  seconds,  with 


a  standard  deviation  of  0.022  seconds.  Figure  [51]  highlights  the  advantage  of  using 
variable  calculation  timing.  If  a  fixed  timing  update  were  selected  based  on  this  data, 
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Figure  51.  Average  Loop  Times  for  Simulation  Runs 


it  would  be  about  A tcaic  =  1.3  seconds,  and  no  trajectory  updates  would  have  been 
available  until  that  time  for  each  epoch.  Additional  complexity  in  the  creation  of 
the  dealer  function  was  required  to  be  able  to  accept  updates  as  soon  as  they  were 
available,  but  59%  more  path  updates  were  received,  greatly  increasing  the  system’s 
flexibility  and  ability  to  deal  with  uncertainty. 

The  system’s  final  error  upon  reaching  the  approach  point  is  shown  in  Figure  [52] 
As  expected,  the  performance  in  the  vertical  component  was  better  than  required, 
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Figure  52.  Final  Target  Estimate  Error  for  Simulation  Runs 

due  to  the  number  of  highly  orthogonal  measurements  for  the  entire  flight  (variance 
of  the  series  of  final  vertical  error  estimates  from  the  1000  simulation  runs  was  0.0026- 
m2).  For  this  geometry,  the  certainty  in  the  horizontal  target  estimate  is  the  critical 
parameter  that  the  path  planner  must  meet.  The  average  of  the  final  horizontal  co- 
variance  estimates  from  the  simulation  runs  was  0.017-m2,  which  closely  matched  the 
actual  variance  of  the  final  horizontal  error  of  0.016-m2.  The  estimated  covariance  re¬ 
quirement  was  to  be  below  the  limit  of  0.02-m2,  but  was  slightly  better  than  expected 
dne  to  the  fact  that  typically  2-3  measurements  come  in  during  each  planning  cycle. 
If  the  first  measurement  is  the  one  that  put  the  variance  under  the  limit,  the  effect  of 
all  three  is  still  recorded,  as  they  are  processed  in  batch.  This  certainty  is  acceptable 
for  landing  considering  the  size  and  shape  of  the  quadrotor’s  arresting  hook,  and  the 
estimate  will  be  improved  with  the  additional  measurements  that  will  come  during 
the  flare  segment  until  the  camera  exits  the  true  FOV  limits.  More  importantly,  how¬ 
ever,  the  result  validates  the  algorithm’s  effectiveness  at  accomplishing  the  primary 
purpose  of  the  research — to  create  a  path  in  real  time  that  can  achieve  a  required 
amount  of  target  position  certainty  in  a  stochastic  environment. 


1000 
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The  time  series  results  of  the  simulation  runs  can  be  seen  in  Figure  [53]  and  Fig¬ 


ure 


54 .  The  extended  times  for  some  runs  were  due  to  a  more  distant  target  location 
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Figure  53.  Target  Position  Estimation  Error  During  1000  Simulation  Runs 


Figure  54.  Target  Covariance  During  1000  Simulation  Runs 


(the  longest  run  was  38-m).  These  results  show  the  stability  and  predictability  of  the 
UFK  algorithm,  and  the  ability  to  achieve  the  final  required  covariance  estimate. 
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9.2  Flight  Test  Results 


The  flight  test  approach  for  the  system  included  a  build-up  series  of  flights  initially 
working  with  the  stability  of  the  system,  followed  by  the  path  tracking  capability. 
Most  of  these  flights  were  accomplished  in  the  small  (4-m  square)  flight  facility  in 


the  AFIT  ANT  Center,  shown  in  Figure  55.  For  tracking,  the  dealer  program  was 


Figure  55.  Flight  Control  Work  Accomplished  in  the  ANT  Center  (Photo:  New  York 
Times) 


incorporated  to  command  simple  flight  profiles,  eventually  adding  the  path  planning 
system.  Further  flights  were  accomplished  to  test  flying  qualities  with  the  arresting 
hook,  which  were  found  to  be  unacceptable  due  to  a  large  vibration  mode  induced  by 
the  flexible  hook.  The  hook  was  dampened  with  a  series  of  support  lines,  and  scaled 
down  profiles  were  flown  to  test  the  flare  segment  profile  and  to  test  engagement  of 
an  actual  wire. 

Full-size  profile  flights  were  first  accomplished  with  a  simulated  wire  in  the  /iAVIARI, 
followed  by  the  final  two  end-to-end  tests  conducted  with  a  real  wire  to  demonstrate 
the  complete  system  from  takeoff  to  perching  on  the  power  line.  The  only  human  in¬ 
put  to  the  system  for  the  full  profile  flights  was  consent  to  turn  the  motors  on  and  off. 
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The  runs  were  initialized  in  the  same  manner  as  the  Monte  Carlo-style  simulation, 
with  the  exceptions  noted  in  Table  |3j 

Table  3.  Flight  Test  Parameters  and  Results 


Parameters 

Run  1 

Run  2 

x0  (m) 

[-8  3  ] 1 

[-8  3  ] 1 

x*  (m) 

[  8.54  4.17  1T 

[  8.54  4.17  ] 

xi0  (m) 

[4  3]T 

[15  5  ]T 

Results 

Avg  Loop  Time 

0.83  s 

0.85  s 

Min  Loop  Time 

0.77  s 

0.77  s 

Max  Loop  Time 

0.92  s 

0.97  s 

RTOC  Segment 

31.53  s 

32.33  s 

*error{tperch )  (ill)  [  0.0117  0.0144  1 

T 

[  0.0247  0.0298  ]T 

P  (tapp)  (m)2 

0.0195  0.0025 

0.0025  0.0024 

0.0185  -0.0024 

-0.0024  0.0024 

9.2.1  Flight  Test  Run  #1. 

It  should  be  noted  that  Flight  Test  Run  #1  and  Run  ^2  were  the  actual  first 
and  second  flights  with  an  installed  wire.  The  complete  flight  path  for  Run  #1  can 
be  seen  in  Figure  [56j  with  the  flight  progressing  from  the  negative  ir-axis  side  of  the 
facility  with  the  origin  placed  near  the  center  of  the  room.  Tracking  was  acceptable, 
with  the  exception  of  the  space  at  the  approach  point,  which  can  be  more  readily 
seen  in  Figure  [57| 

The  cause  of  the  deviation  is  the  slow  integrators  on  position  error,  and  the  fact 
that  the  system  was  tracking  the  optimal  path  with  feedback  vice  using  feed-forward 
of  the  optimal  control.  The  run  starts  at  t  =  30  seconds,  when  the  first  measurement 
is  accepted  and  the  RTOC  system  is  engaged.  Path  error  in  the  ^-direction  increases 
as  the  command  “leaves”  the  hovering  vehicle  and  it  begins  to  catch  up.  While  the 
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vehicle  travels  across  the  room,  the  integrator  adds  up  the  difference  to  remove  the 
steady-state  error,  only  to  overshoot  as  the  vehicle  nears  the  approach  point  and 
the  horizontal  speed  command  abruptly  stops.  For  future  systems  that  have  more 
of  an  ability  to  flight  test  the  control  system,  the  speed  of  the  integrators  should 
be  increased  to  lessen  the  amount  of  time  that  steady-state  errors  are  present  within 
reason  for  strong  stability.  In  addition,  to  anticipate  the  “corners”  in  the  flight  profile, 
a  feed-forward  element  should  be  added  to  the  feedback  error  loop  guided  by  the 
actual  optimal  control  time  history.  This  will  change  the  control  prior  to  “corners” 
for  better  (perfect,  in  theory)  tracking  of  the  path.  As  is,  the  system  is  guided  by  the 
error  between  the  current  and  optimal  paths,  which  will  always  result  in  late  control 
inputs,  as  nothing  happens  until  the  paths  have  already  begun  to  diverge. 

The  shell  profile  for  the  flight  is  most  easily  seen  in  the  ^-direction  (vertical).  The 
aircraft  takes  off  and  is  directed  to  hold  at  1-m  to  check  stability,  taking  a  moment 
to  integrate  the  vertical  steady-state  control  requirement  as  it  begins  to  leave  ground 
effect.  After  a  5  second  hold,  the  aircraft  is  directed  to  a  hover  at  the  start  run  point. 
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Figure  57.  Commanded  vs  Actual  Flight  Path,  Flight  Test  Run  #1 

The  RTOC  portion  of  the  run  is  from  t  —  30  seconds  until  t  =  61.5  seconds,  at  which 
point  the  vehicle  is  held  level  and  slowly  moves  forward  to  the  wire.  There  is  no  actual 
sensor  on  the  vehicle  to  detect  the  wire — the  vehicle  stops  based  on  the  last  known 
relative  position,  as  the  wire  is  no  longer  in  the  FOV.  These  are  obvious  difficulties 
that  should  be  remedied  in  a  full  system.  Tracking  is  good,  and  the  separation  of 
the  vertical  paths  is  seen  at  the  point  where  the  wire  is  actually  in  contact  with  the 
hook,  denoted  by  the  vertical  red  line  in  each  of  the  plots.  At  this  point,  the  vertical 
command  continues  to  descend,  but  the  vehicle  stops  as  soon  as  the  slack  is  taken  out 
of  the  wire.  The  engines  are  turned  off  at  115  seconds,  as  noted  by  the  quick  vertical 
drop  as  the  wire  stretches  slightly  with  the  remaining  vehicle  weight. 

Heading  (?/>)  error  is  minimal,  once  stabilized,  with  some  difficulty  during  the 
slower  flare  portion  of  the  path.  The  horizontal  position  command  during  the  flare 
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portion  is  not  linear,  but  a  continually  slowing  path  as  the  wire  is  approached.  With 
the  heading  controlled  by  the  torque  balance  of  the  engines,  the  constant  small 
changes  in  pitch  angle  required  to  track  the  path  and  its  constant  speed  changes 
resulted  in  some  difficulty  maintaining  heading,  and  y- axis  error,  which  occurred 
right  at  the  wire  and  may  have  also  been  induced  somewhat  by  propwash  from  the 
nearby  wall. 

9.2. 1.1  RTOC  Performance. 

The  RTOC  system  performed  exactly  as  designed.  The  actions  of  the  path  plan¬ 
ning  system  as  it  converges  to  the  optimal  path  are  difficult  to  characterize  without  a 
string  of  all  of  the  system  updates,  but  Figure  [58]  summarizes  this  with  a  progression 
of  instantaneous  solutions  in  the  vertical  plane  at  separate  sample  times.  The  arrows 
from  the  vehicle  denote  the  actual  bearing  measurements  received  by  the  system,  and 
the  directions  give  a  sense  of  the  magnitude  of  the  measurement  errors  (they  should 
point  through  the  true  target).  Both  the  estimated  and  actual  target  location  can 
be  seen.  The  covariance  ellipsoid  shows  a  95%  likely  confidence  ring,  and  the  error 
in  the  initial  seconds  exceeds  this  slightly  as  the  estimate  settles  with  the  first  few 
measurements.  The  diamonds  denote  target  estimate  histories,  showing  a  trend  to¬ 
ward  the  true  target  with  an  unsurprising  difficulty  in  resolving  range.  The  range 
ambiguity  can  also  be  seen  in  the  orientation  of  the  covariance  ellipse,  which  has  the 
greatest  uncertainty  in  the  direction  of  the  LOS  from  the  vehicle.  The  reason  for 
the  “hook”  at  the  end  of  the  paths  is  clearly  seen,  as  the  path  planner  moves  the 
vehicle  to  a  position  orthogonal  to  the  greatest  axis  of  uncertainty  remaining.  The 
last  measurements  in  the  profile  are  critical,  both  in  terms  of  the  value  of  close  range 
measurement  and  the  value  of  measurements  from  that  direction. 
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Figure  58.  Flight  Test  Run  #1  Snapshots,  and  Comparison  to  Full-Knowledge  Path 


The  final  panel  of  Figure  58  shows  the  comparison  of  the  actual  path  that  was 
flown  by  the  vehicle  with  the  path  that  would  have  been  commanded  had  the  target 
position  estimate  always  been  perfectly  accurate.  This  demonstrates  the  true  power  of 
stochastic  real-time  optimal  control.  Even  with  the  initial  error  in  the  target  position, 
and  with  the  errors  in  each  measurement,  the  actual  path  that  the  vehicle  flew  was 
very  close  to  the  perfect- information  solution. 


9.2.2  Flight  Test  Run  #2. 

As  previously  mentioned,  the  initial  target  estimate  for  the  second  test  flight  profile 
was  set  unrealistically  high,  making  the  “low  road”  the  global  minimum  due  to  the 
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insufficient  observability  of  the  horizontal  axis  while  near  the  maximum  allowable 


altitude.  The  flight  path  is  shown  in  Figure  59 


The  comparison  of  commanded  vs. 


Figure  59.  Flight  Path,  Flight  Test  Run  #2 


actual  position  can  be  seen  in  Figure  [60}  As  can  be  seen,  Run  #1  and  Run  #2 
exhibited  many  of  the  same  characteristics. 

The  RTOC  controller  performance  is  shown  in  the  snapshot  progression  of  Fig- 
61  Note  the  position  of  the  target  estimate  in  Figure  [61}]  in  relation  to  the 
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maximum  allowed  altitude.  This  is  what  forced  the  “low  road”  to  be  the  optimal 
path  with  the  initial  information.  Even  though  the  estimate  had  moved  down  signifi¬ 
cantly  by  Figure  |6T}j,  once  the  vehicle  has  committed  to  a  certain  direction,  switching 
to  the  local  minimum  on  the  other  side  becomes  too  costly.  In  terms  of  mission  ac¬ 
complishment,  the  only  loss  from  the  perfect-information  solution  in  this  contingency 
case  is  a  small  increase  in  flight  time,  0.8  seconds  over  that  of  Run  #1. 

The  final  panel,  Figure  [6l]i,  shows  the  path  of  the  flare  mode,  which  proceeds 
level  from  the  approach  point  to  the  perch  point  before  commanding  a  descent  to 
engage  the  hook,  as  shown  in  Figure  [62} 


163 


Figure  60.  Commanded  vs  Actual  Flight  Path,  Flight  Test  Run  #2 
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Figure  61.  Snapshot  Progression  of  Flight  Test  Run  #2 


Figure  62.  Quadrotor  Just  Prior  to  Hook  Engagement 
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X.  Conclusions  and  Future  Work 


£  f  HIS  research  successfully  developed  a  method  to  simultaneously  solve  the  op- 
timal  control  and  the  optimal  estimation  problems.  A  recursive  algorithm 
was  designed  to  implement  the  method  in  real-time  for  disturbance  rejection  and 
treatment  of  uncertainties  in  the  model  and  measurements.  The  solution  is  compre¬ 
hensive,  and  was  verified  in  flight  test — autonomously  landing  a  quadrotor  helicopter 
on  a  wire  as  an  enabler  for  the  future  capability  of  energy  harvesting.  This  method 
may  be  applied  to  any  system  with  a  bearing-only  sensor  that  requires  relative  posi¬ 
tion  information  about  a  source  in  order  to  perform  its  primary  mission. 


10.1  Conclusions 

The  most  obvious  conclusion  from  this  research  is  that  a  vehicle  may  be  guided 
to — and  landed  upon — a  wire  using  stochastic,  delayed,  bearing-only  measurements. 
Future  systems  that  may  benefit  from  energy  harvesting  are  sensor  limited,  and  most 
systems  of  such  size  have  only  a  monocular  camera.  Additional  sensors  may  be 
desirable  for  landing  on  power  lines,  but  are  not  required.  Furthermore,  this  study 
provides  support  that  the  Unscented  Kalman  Filter  is  a  suitable  estimation  tool  for 
such  applications,  and  that  real-time  optimal  control  may  be  applied  to  direct  a  path 
that  will  acquire  the  level  of  target  position  certainty  necessary  to  commit  to  a  landing 
maneuver. 

In  the  realm  of  trajectory  optimization,  several  conclusions  can  be  drawn  from 
these  efforts.  The  first  is  that  there  is  a  fundamentally  different  way  to  approach 
the  localization  and  dual  control  problems  that  is  more  suitable  and  effective  than 
traditional  methods.  The  ubiquitous  technique  of  optimizing  a  cost  functional  corn- 
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prised  of  a  scalar  approximation  of  a  multi-dimensional  certainty  metric  has  several 
disadvantages  that  are  overcome  with  the  methods  developed  in  this  work. 

In  the  new  approach,  a  user  retains  the  directional  information  that  was  formerly 
approximated  by  a  scalar,  dispensing  with  difficulties  of  randomly  odd-shaped  uncer¬ 
tainty  ellipsoids  and  other  effects  of  information  compression.  This  allows  shaping  of 
the  uncertainty  to  match  the  physical  requirements  of  the  system,  such  as  the  actual 
shape  of  an  arresting  hook  on  a  sUAS.  Furthermore,  previous  methods  minimized 
current  uncertainty  as  much  as  possible,  vice  to  a  specific  level.  This  research  has 
now  provided  a  way  to  prescribe  the  filial  uncertainty,  which  is  the  true  requirement 
for  mission  accomplishment.  Without  this  ability,  a  vehicle  will  maneuver  as  much 
as  it  can  until  an  arbitrary  time,  or  perhaps  will  balance  the  amount  of  maneuvering 
based  on  some  arbitrary  weight  on  the  current  certainty.  Either  way,  it  will  not  know 
whether  it  will  achieve  the  necessary  amount  of  target  information,  or  whether  it 
has  wasted  effort  collecting  too  much  information  until  the  vehicle  reaches  the  point 
where  the  information  is  required,  when  it  is  too  late. 

Early  efforts  provided  a  shooting  solution  which  would  allow  a  user  to  prescribe 
a  final  covariance.  Trial  solutions  would  be  checked  for  the  expected  final  covari¬ 
ance,  iterating  the  weighed  cost  functional  until  the  path  produced  yielded  the  right 
size  and  shape  final  certainty.  This  method  was  eclipsed  by  an  elegant,  single-shot 
solution  that  simultaneously  handles  the  optimal  control  desires  without  weighting 
adjustments  while  meeting  the  physical  information  needs  of  the  system.  The  single- 
shot  solution  was  made  possible  by  augmentation  of  the  system  state  vector  with 
states  that  contain  an  estimation  in  the  polynomial  space  of  the  knowledge  gained 
by  the  constellation  of  discrete  measurements  normally  expressed  by  the  Fisher  In¬ 
formation  Matrix.  Dynamics  were  developed  for  these  information  states,  and  with 
care  to  avoid  singularities,  boundary  conditions  were  enforced  to  ensure  that  by  the 
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time  the  system  arrives  at  the  desired  final  state,  it  will  have  collected  the  appro¬ 
priate  measurements,  from  the  necessary  angles,  to  finish  the  flight  with  the  desired 
certainty  in  the  target  location  estimate. 

A  further  conclusion  drawn  is  that  the  requirement  of  fixing  a  final  time  in  the  dual 
control  problem  can  now  be  changed  to  a  free  final  time.  This  was  previously  done 
either  explicitly,  or  implicitly,  through  methods  such  as  fixing  a  final  distance  with  a 
given  closure,  fixing  the  total  number  of  measurements  with  a  given  update  rate,  or 
by  fixing  an  allowable  travel  proportion  of  the  estimated  distance  to  the  target  (with 
a  constant  speed).  A  fixed  final  time  is  a  significant  limitation  for  application  to  real 
systems  beyond  simulation.  In  reality,  the  time  that  will  elapse  during  maneuvers  not 
yet  solved  for  is  unknown,  as  is  the  number  of  measurements  that  will  be  required  to 
meet  the  final  mission  requirements.  The  proportion  of  distance  relative  to  the  initial 
unknown  distance  is  obviously  also  unknown.  Choosing  any  of  these,  or  more  directly 
just  choosing  the  fixed  final  time  ends  up  being  a  primary  driver  of  the  characteristics 
of  the  solution  trajectory.  A  solution  that  is  truly  optimal  must  be  able  to  vary  the 
problem  geometry  to  get  the  required  number  of  measurements  from  the  necessary 
angles  to  accomplish  the  mission  without  limiting  the  set  of  possible  solutions  to  those 
paths  which  end  at  a  particular  final  time. 

Several  conclusions  can  also  be  drawn  in  the  area  of  RTOC.  The  successful  appli¬ 
cation  of  a  recursive  algorithm  with  pseudospectral  methods  as  the  engine  working 
sequentially  with  a  UFK  receiving  measurements  from  a  bearing-only  source  is  of 
great  benefit.  It  validated  the  theory  of  disturbance  rejection  and  the  ability  to  use 
the  speed  of  the  pseudospectral  methods  to  produce  solutions  that  can  guide  in  real¬ 
time.  Applying  the  theory  to  real  hardware  produced  several  tools  that  were  not 
required  in  previous  PSM  RTOC  simulations,  such  as  an  intermediate  function  to  ad- 
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dress  the  asynchronous  timing  loops  between  a  control  system  and  an  unpredictable 
optimal  solver. 

This  work  clearly  demonstrated  that  allowing  the  calculation  time  of  the  optimal 
solver  to  vary  has  great  value,  increasing  the  flexibility  and  responsiveness  of  the 
system  by  increasing  the  rate  of  available  optimal  solutions.  The  structure  necessary 
to  address  the  potential  discontinuities  that  result  from  achieving  this  benefit  was  also 
designed  and  implemented,  using  a  blending  solution  to  ensure  smooth  and  accurate 
control  with  the  most  current  data  from  both  the  path  planner  and  the  estimation 
filter. 

From  a  systematic  perspective  of  basic  RTOC  implementation,  this  research  showed 
that  the  trend  in  the  RTOC  community  of  equating  closed-loop  feedback  control  with 
a  fast,  recursive  optimal  solution  is  insufficient.  This  conclusion  has  developed  over 
time  as  a  byproduct  of  most  of  the  RTOC  applications  being  limited  to  simulation. 
Non-zero  mean  and  time-correlated  biases  will  cause  steady-state  errors  that  will  be 
unaccounted  for  by  a  purely  feed-forward  solution.  Though  such  a  system  will  reach 
the  final  condition,  the  optimality  of  the  path  it  takes  is  more  of  a  mathematical 
construct  than  an  operational  reality.  A  more  comprehensive  and  effective  method 
must  account  for  the  anticipated  future  effects  of  disturbances  and  model  inaccura¬ 
cies.  This  can  be  done  through  classical  integration  of  the  error  between  the  expected 
and  actual  paths,  and  through  feedback  of  disturbance  estimates  into  the  dynamical 
model  for  each  optimal  solver  epoch.  The  ideal  RTOC  structure  is  to  accomplish 
both,  updating  the  model  with  estimates,  and  applying  a  total  control  solution  that 
is  a  combination  of  the  open-loop  optimal  solution  and  an  integrated  error  feedback 
component. 

Finally,  this  research  provides  a  planning  tool  that  may  be  used  to  develop  heuris¬ 
tics  for  a  suboptimal  approach  to  landing  on  a  power  line  that  may  be  sufficient  for 
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systems  with  significant  computational  limitations,  as  may  be  the  case  for  many  sUAS 
platforms.  Re-creating  the  single-shot  solution  with  the  particular  system  dynamics 
and  limitations  would  provide  the  characteristics  common  to  optimal  solution  paths. 


10.2  Future  Work 

There  are  many  directions  of  research  that  can  be  pursued  from  this  point.  The 
likely  fielded  implementation  of  a  full  RTOC  path  planner  is  for  submarine  guidance. 
To  modify  the  problem,  the  axes  must  first  be  simply  rotated  into  the  horizontal. 
A  study  should  be  made  to  determine  whether  adding  a  third  dimension  would  be 
beneficial  or  not,  based  on  the  ratio  of  relative  pickup  ranges  to  vertical  maneuvering 
ability.  Previous  research  has  decided  that  it  is  not  necessary,  but  if  it  is  added,  the 
information  states  will  need  to  be  increased  to  6  elements,  and  if  the  final  covari¬ 
ance  is  still  the  required  parameter  of  choice,  a  differentiable  method  for  solving  or 
approximating  the  3-dimensional  FIM  inverse  will  be  required. 

To  incorporate  the  likelihood  of  a  moving  target,  the  estimation  filter  must  be 
expanded  to  include  states  for  target  velocity  and  target  heading.  The  RTOC  prob¬ 
lem  can  accept  these  as  constants,  and  plan  the  path  based  on  the  assumption  that 
the  target  will  not  maneuver.  If  future  maneuvers  do  occur,  the  path  planner  will 
recursively  solve  the  problem  with  the  best  information  it  has  at  the  time.  There  are 
open  questions  along  this  direction,  such  as  observability  requirements  (much  like  the 
power  line  problem,  with  both  range  and  speed  unknown,  you  can  receive  the  same 
bearing  measurements  for  infinite  paths  unless  the  observer  maneuvers).  In  addition, 
adding  a  second  measurement  source  for  a  towed  array,  and  a  velocity  input  from 
Doppler  measurements  would  make  the  solution  fieldable. 
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For  the  sUAS  problem,  the  optical  requirements  remain  unaddressed.  An  op¬ 
tical  line  detection  algorithm  should  be  implemented,  either  new,  or  with  existing 
technologies.  This  could  also  be  expanded  to  include  stadiametric  ranging,  since  the 
approximate  height-above-ground  may  be  known  for  the  power  line  or  the  utility 
poles.  Identification  of  utility  poles  in  the  image  would  also  be  of  benefit.  If  the 
problem  can  detect  lateral  motion  in  relation  to  the  line,  then  lateral  motion  will  im¬ 
prove  observability,  and  the  3-dimensional  FIM  should  be  incorporated  as  discussed. 
To  incorporate  the  ability  to  land  upon  a  ledge  or  other  perch,  only  the  optical  re¬ 
quirements  for  determining  an  appropriate  landing  site  change  (and  the  flare  segment, 
obviously).  The  approach  segment  method  used  in  this  work  can  be  used  interchange¬ 
ably,  with  the  safety  stand-off  distance  used  herein  to  avoid  hitting  a  window  or  other 
structure  by  commanding  no  flight  past  a  safe  limit  until  the  range  to  the  ledge  is 
sufficiently  certain.  The  size  of  the  safety  limit  can  shrink  in  accordance  with  the 
current  certainty  level  for  that  epoch. 

The  flight  test  can  also  be  expanded  for  realism.  Adapting  the  system  to  a  fixed- 
wing  asset  and  accomplishing  the  flight  test  outdoors  on  a  full-sized  power  line  would 
obviously  be  ideal,  and  would  drive  solutions  for  more  significant  disturbance  rejec¬ 
tion,  especially  in  the  landing  phase.  If  the  same  quadrotor  is  used,  the  hook  should 
be  redesigned  with  an  “open  mouth”  that  will  allow  it  some  vertical  error,  and  a 
way  to  detect  line  engagement,  so  that  it  may  “drive  through”  the  line  estimate,  and 
not  need  to  know  it  so  precisely.  The  flight  control  system  must  also  be  improved, 
using  feed-forward  optimal  control  inputs  with  feedback  of  trajectory  error.  Lastly, 
the  power-to-weight  problem  of  the  device  used  to  recharge  the  battery  inductively 
needs  to  be  addressed,  as  current  solutions  are  too  heavy  for  very  light  platforms. 
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10.3  Summary 


In  summary,  this  work  provided  a  method  that  can  be  applied  to  a  system  with 
any  given  dynamics,  and  with  any  cost  function,  that  will  allow  it  to  be  guided  in 
relation  to  an  estimated  target  location  to  accomplish  a  potentially  unrelated  primary 
control  mission.  Deviations  from  the  optimal  path  will  be  made  to  collect  bearing- 
only  measurements  in  sufficient  quantity  and  with  a  sufficient  angular  orthogonality 
to  identify  the  target  location,  without  wasting  maneuver  effort  beyond  the  minimum 
necessary  to  provide  the  level  of  certainty  in  the  target  location  estimate  that  is  re¬ 
quired  for  mission  accomplishment.  This  method  can  be  modified  to  apply  it  towards 
guidance  of  submarines  using  passive  sonar,  HARM  missiles,  or  other  bearing-only 
systems.  For  the  future  capability  of  energy  harvesting,  the  system  can  guide  a  sUAS 
from  a  point  with  an  initial  bearing  measurement  to  a  power  line  to  an  approach 
point  from  which  a  flare  maneuver  can  be  commenced  for  landing.  With  the  current 
capability  to  autonomously  guide  a  system  to  a  location  where  a  power  line  can  be 
found,  and  the  current  research  in  the  area  of  the  actual  flare  maneuver,  this  research 
makes  full-scale  landing  on  a  power  line  a  near-term  technology. 


Figure  63.  Engine  Shutdown 
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Appendix  A.  Quadrotor  Flight  Control  Model 


/The  simulator  developed  for  the  quadrotor  helicopter  is  preserved  in  the  Simulink 
diagrams  of  this  appendix.  Direction  cosine  matrices,  equations  of  motion, 
and  some  other  features  that  are  either  clear  by  context  or  covered  in  the  main  body 
of  the  dissertation  are  omitted. 


A.l  Simulink  Model 

The  main  flow  of  the  simulator  can  be  seen  in  the  top-tier  diagram  of  Figure  |64| 
A  commanded  path  is  generated  either  to  judge  performance  and  stability  with  step 
functions  and  the  like,  or  to  input  a  commanded  profile  from  the  path  planner  to 
test  tracking  performance.  Testing  of  the  hover  mode  was  performed  with  the  next 
block  (Figure  |65j)  to  ensure  that  the  system  would  lock  a  current  commanded  position 
when  a  button  on  the  hand  controller  was  pressed  and  released  (initiation  happens 
on  the  “release  frame”  vice  the  “press  frame”  to  avoid  multiple  actuations).  Initial 
conditions  also  must  be  compensated  for  in  the  hover  block  for  use  with  the  automated 
flight  mode.  For  the  hand  control  mode,  if  no  command  is  made,  the  aircraft  should 
not  move  from  the  place  the  engines  are  started  (else  the  aircraft  would  jump  to 
the  navigation  frame  origin).  Initial  conditions  are  therefore  added  as  offsets  to  the 
hand  control  commands.  Since  this  happens  “downstream”  in  the  code,  the  additive 
inverse  is  added  during  automated  flight  to  cancel  the  effect  out  and  ensure  that  the 
aircraft  flies  to  the  actual  navigation  frame  input.  The  automated  commands  for  the 
aircraft  are  derived  in  real-time  to  ensure  the  shell  always  starts  from  the  vehicle’s 
true  initial  position. 

The  zero-order-hold  blocks  in  Figure  |64|  discretize  the  model.  Commands  are  dis¬ 
crete  in  order  to  use  the  same  transfer  functions  as  are  required  in  the  true  controller, 
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Figure  65.  Logic  to  Initiate  Hover  Mode,  Lock  in  Current  Position,  and  Compensate 
for  Initial  Conditions 


but  equations  of  motion  are  all  treated  continuously.  The  first  DCM  transforms  the 
commanded  coordinates  from  the  hand  controller  axes  to  the  navigation  frame,  based 
on  the  position  the  observer  pilot  expects  to  stand  in  relation  to  the  room.  Com¬ 
manded  position  is  then  compared  to  expected,  and  the  error  signal  is  saturated,  with 
different  levels  in  each  axis,  to  control  the  maximum  velocity  as  discussed  in  Chap¬ 


ter  VIII  The  resultant  error  signal  is  then  transformed  into  the  body  frame  and  sent 
to  the  ground  station  controller,  which  generates  control  signals  for  the  inner  loop 


controller  on  board  the  aircraft,  as  shown  in  Figure  66  Ground  station  control  laws 


for  each  axis  are  shown  in  Figures  67  69  Integration,  sneakback,  and  anti-chatter 


logic  are  shown  for  the  x-direction  in  Figures  70  72  The  logic  is  the  same  for  the 
?/-axis,  and  is  similar  in  the  x-axis,  which  includes  integration  and  reset  logic,  but  does 
not  require  sneakback  or  anti-chatter.  The  control  signal  from  the  ground  station  is 
sent  to  the  servo-sensor  board  on  the  aircraft,  which  is  modeled  in  Figure  [73}  This  is 
the  inner  stabilization  loop,  and  it  contains  an  input  to  simulate  IMU  noise,  as  well 
as  the  discrete  lag  filters  used  to  estimate  angular  accelerations  based  on  the  angular 
rate  commands.  The  commands  for  each  axis  are  combined  in  a  mixer  to  determine 
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the  actual  motor  commands.  The  mixer  is  easier  to  understand  in  equation  form: 


MotorOcmd 

1  1-11 

PcmcL 

Motor  lcmd 

-1111 

Qcmd 

Ah  otor  2 cmd 

1 

I 

1 

^  cmd 

M  otor  3cmd 

1-111 

throttle  cmd 

(169) 


where  the  motors  are  numbered  clockwise,  starting  with  zero  in  the  front  left  corner 
when  facing  in  the  positive  x  direction. 


The  forces  and  moments  are  determined  in  Figure  74  using  an  assumed  (not 
measured)  first-order  model  for  motor  spin-up  delay  with  the  engine  thrust  and  torque 
models: 


Torque  (N  -  m)  =  4.16029e“5(PWM)2  -  0.09592(PWM)  +  55.49559  (170) 

Thrust  (N)  =  6.78e“6(PWM)2  -  0.009868(PWM)  +  2.90352  (171) 


Each  motor  acts  0.15-m  from  the  centerline  for  purposes  of  calculating  the  actual 
moments.  With  the  forces  and  moments,  the  position  and  orientation  of  the  aircraft 
is  solved  for  using  Equations  156||163  in  Chapter  VIII  Some  delay  can  be  added 
to  the  position  and  orientation  to  provide  the  discrete  Vicon  measurements,  but  in 
practice  this  was  found  to  be  negligible.  Finally,  the  measurements  are  rotated  into 
the  navigation  frame  to  complete  the  top-tier  loop.  Gain  values,  constants,  and  other 
specific  details  can  be  found  in  the  initialization  hie  in  Table  |4j 
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Figure  67.  Horizontal  Control  Laws 


Figure  68.  Vertical  Control  Law 


Figure  69.  Heading  Control  Law 
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Sneakbackand  anti-chatter 


Logic  to  drive  avitch:  If  error  near  limit, 
sneakbackto  zero,  el9e  integrate 


Switch:  Sneakoack 
or  Integrate 

=fi 


CD — ► 

Toggle  Motors 

m — ► 


x  int  sat 


Anti-Windup:  Reset 
integrator  when  motorsare  off 


>C D 

x  int 


Figure  70.  Integrator  Management  Logic — Integrate,  Reset,  Anti-windup,  Anti- 
Chatter  (Also  Used  for  y- Axis) 


Integrate  In 


Figure  71.  Subsystem  for  Switch  Logic — Integrate  if  within  Limits,  Else  Pull  Integrator 
Back 


sneak  back  factor  x 


ik 


Q> 

x  int 


Logic:  Pull  integrated  value  toward  zero, 
stop  If  nearzeroto  avoid  command  chatter 


► 

i 

Switch  threshold  at 
sneak_back_factor 


t 

- ► 

Switch  threshold  at 
-sn  e  a  k_b  a  ck_f  a  cto  r 


-KjD 

sneak  to  zero 


Figure  72.  “Sneakback”  and  Chatter  Avoidance  Subsystem 
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Figure  73.  Inner  Control  Loop  (Servo  Sensor  Board  Model  with  Simulated  Noise  Input) 
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Table  4.  Simulator  Initialization  and  Constants 


Parameter 

Value 

Comments 

g 

9.81  m/sz 

m 

1.8  kg 

Mass,  Batteries  Installed 

dt 

0.01  s 

Ground  Station  Time  Step 

Vicon.delay 

0.1  s 

Conservative  Delay  Estimate 

I XX 

0.0203  kg-m 

2  Mass  Model 

lyy 

0.0205  kg-m 

2 

1 ZZ 

0.04  kg-m2 

k_psi_p 

4000 

Angle  Feedback 

k_theta_p 

6 

k  phi  p 

6 

k_Px_p 

4000 

Proportional  Error  Feedback 

k-Py-P 

4000 

k_Pz_p 

150 

k_Vx_p 

1750 

Velocity  Damping 

k_Vy_p 

1750 

k_Vz_p 

100 

kp-P 

12 

Inner  Loop  (SSB)  Angle  Rate  Feedback 

kq_p 

12 

kr_p 

12 

kp_d 

180 

SSB  Angular  Acceleration  Feedback 

kq_d 

180 

kr  d 

180 

maxjx.vel 

1  m/s 

Speed  Limits 

max_y_vel 

1  m/s 

max_z_vel_up 

1  m/s 

max  z  vel  down 

1  m/s 

nominaLthrot 

1680 

T 

-1-  motor 

0.03  s 

Lag  Constant  (Estimated) 

IMLLGYRO  .SCALE  0.0091575 

Least  Significant  Bit  to  Fixed  Point 

bmrot 

46 

Input  Matrix  for  Angular  Accel  Estimate 

plii  mot 

210 

Transition  Matrix  for  Angular  Accel  Est. 

Continued  on  next  page. . . 
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(continued) 


Parameter 

Value 

Comments 

recovery  .time 

10  s 

Controls  to  Tune  Integrator  Speed 

recovery  .time  _z 

7.5  s 

recovery  .time  .yaw 

10  s 

Integrator  Saturation  and  Gain  Values  are  Solved  for  Based  on  the 
Recovery  Time  to  Produce  the  Desired  Maximum  Velocities 
kx_int  =  kPx_p/recovery_time/50  Adjusted  for  50-Hz  Controller 
kyJnt  =  kPy  p /recovery  time/50 
kzJnt  =  kPz  p /recovery  time  z/50 
kpsi_int  =  k_psi_p/recovery_time_yaw/50 

x_pos_sat  =  1.5  *  maxjx.vel  *  kVx_p  /  kPx_p 
y  pos  sat  =  1.5  *  max  y  vel  *  kVy.p  /  kPy.p 
z  pos  sat  up  =  max  z  vel  up  *  kVz.p  /  kPz.p 
z_pos_sat_dn  =  max_z_vel_dn  *  kVz_p  /  kPz_p 

x_int_sat  =  x_pos_sat  *  kPx_p  /  kxJnt 
y  int  sat  =  y  pos  sat  *  kPy.p  /  kyJnt 
z  int  sat  up  =  z  pos  sat  up  *  kPz.p  /  kz  int 
z_int_sat_dn  =  z_pos_sat_dn  *  kPz_p  /  kz_int 
psi_int_sat  =  7r/2  *  k_psi_p  /  kpsi_int 

sneak  back  x  =  0.5  *  x  int  sat  /  50  /  recovery  time 
sneak  back  y  =  0.5  *  y  int  sat  /  50  /  recovery  time 
sneak_back_z  =  0.5  *  z_int_sat_dn  /  50  /  recovery  .time 


Gains  will  convert  to  Fixed  Point  Units  (balance  may  look  wrong) 
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Appendix  B.  Selected  Matlab®  Code 


/  I  OR  length  considerations,  the  complete  code  required  for  the  system  cannot 

t  be  presented  here,  but  a  few  items  are  which  may  be  of  particular  interest. 

The  main  path  planning  loop  is  shown  to  aid  in  understanding  of  the  flow,  and 
the  initialization  section  walks  a  user  through  the  lengthy  connection  process  to  get 
the  path  planning  computer  networked  and  synchronized  with  the  ground  station 
computer,  Vicon,  and  the  aircraft.  This  may  be  useful  to  those  who  would  like  to 
apply  any  similar  external  control  system  to  the  ground  station  for  automated  flight 
control  in  the  ANT  Center  or  the  /iAVIARI,  regardless  of  the  specific  vehicle.  The 
functions  required  to  interface  with  QVOVS  are  also  presented,  as  the  format  may 
be  of  particular  use  to  future  researchers  that  may  require  the  software  for  other 
applications.  All  of  the  further  path  planner  subroutines  are  omitted,  as  they  are 
relatively  application  specific.  The  current  version  of  the  quadrotor  flight  code  is  also 
not  presented,  as  it  is  a  work  in  progress  and  will  shortly  be  obsolete. 


B.l  Main  Path  Planner  Loop 

The  main  path  planner  loop  guides  the  processes  of  calculation  and  communication 
between  the  flight  control  software  and  the  optimal  path  planning  software.  Future 
control  time  histories  are  passed  to  the  dealer  function,  which  parses  the  history 
and  feeds  the  correct  heading  and  position  commands  to  the  flight  controller.  In 
return,  the  dealer  function  provides  the  current  position  of  the  vehicle,  as  well  a  list 
of  recent  angle  measurements  from  the  vehicle  to  the  wire.  The  path  planner  then 
iteratively  calls  the  estimation  filter  and  the  optimal  control  solver  to  update  the 
target  estimate  and  the  optimal  path,  handling  projection  for  initial  conditions  and 
expected  covariance,  path  blending,  and  data  recording. 
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The  optimal  trajectory  provided  by  the  solver  is  spliced  into  the  complete  control 
history  “shell,”  which  also  includes  segments  for  the  takeoff  sequence,  the  flare  mode, 
and  a  backup  landing  mode  in  case  the  wire  is  not  engaged  with  the  current  plan 
and  a  further  plan  is  never  provided.  Scaling  of  the  path  (and  of  the  measurements) 
is  provided  by  subroutines  so  that  the  vehicle  may  operate  in  both  the  ANT  Center 
and  the  larger  /iAVIA  R I  using  the  same  planning  algorithm. 

Communication  between  the  path  planner  and  the  dealer  function  is  accomplished 
by  way  of  a  TCP  connection  that  relies  on  a  TCP/UDP/IP  toolbox  created  by  Peter 
Rydesater  and  can  be  downloaded  from  the  Matlab®  Central  File  Exchange  at: 
http://www.mathworks.com/matlabcenterl/hleexchange/345.  The  toolbox  is  built 
with  .mex  hies,  and  a  .dll  hie  must  hrst  be  created  with  any  C  compiler.  The  com¬ 
munication  tools  provide  the  ability  to  pass  the  data  using  the  “blocking”  techniques 
discussed  in  Section  7.1.3.1[  allowing  variable  calculation  timing.  Breaking  the  mes¬ 
sage  into  TCP  packets  and  reshaping  them  in  the  dealer  function  is  performed  by  a 
subroutine  written  by  Mr.  Mark  Smearcheck. 


Main  Path  Planner  Loop  Code. 


0001  •/.•/.  7 ;%%7„%y//„7»y.y.%7//.%7//.7.7//.7.7.7.y//»7//oyoy//////.7o0/.0/.%7.0/.7.0/.%%%7o%0/o%7//.7.%%7.7.7o7.0/„%7.7.7o%0/o%7//.7.%%7.7.yo% 
0002  7.7.  Main  Quadrotor  Shell  7«7o 

0003  7.7.  7.7.7.7o7o7o7.7o7o7o7o7o7.7.7o7o7o7o7o7o7o7o7o7o7o7o7o7o7o7.7.7o7o7o7.7o7o7o7.7.7o7.7o7o7.7.7o7o7o7o7o7.7o7o7o7.7.7.7o7o7o7o7.7o7.7o7.7o7.7.7o7o 


0004  7«This  is  the  main  program  for  flying  the  quadrotor,  and  it  is  the  main  loop 
0005  7«for  timing  and  communication  between  the  Vicon/Groundstation,  the  optimal 
0006  7.path  planner,  and  the  UKF.  It  can  also  run  in  simulation  mode  without 
0007  7«input  from  the  Vicon  and  the  vehicle.  Running  the  program  will  walk  a 
0008  7«user  through  the  pre-flight  initialization  and  connection  process. 

0009  7. 


0010  7«Written  by  LtCol  Steven  Ross,  AFIT,  2011. 

0011 

0012 


0013  clear  all; 
0014  home 


0015  global  WHERE_T0_RUN  WHAT_T0_RUN  FASTMODE  CONST 

0016  f igure.cascade  =  1;  7.Compare  multiple  figure  sets 

0017 

0018  7.7.  7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7. 

0019  7.7.  Flight  Mode  options  7.7. 

0020  7.7.  7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7. 

0021  sim  =0;  7.1=simulation  mode,  0=f light  mode 

0022  display _on  =  0;  7.1=display  on  in  real-time  (disrupts  timing) 
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0023 

0024 

0025 

0026 

0027 

0028 

0029 

0030 

0031 

0032 

0033 

0034 

0035 

0036 

0037 

0038 

0039 

0040 

0041 

0042 

0043 

0044 

0045 

0046 

0047 

0048 

0049 

0050 

0051 

0052 

0053 

0054 

0055 

0056 

0057 

0058 

0059 

0060 

0061 

0062 

0063 

0064 

0065 

0066 

0067 

0068 

0069 

0070 

0071 

0072 

0073 

0074 

0075 

0076 

0077 

0078 

0079 

0080 

0081 

0082 

0083 

0084 

0085 

0086 

0087 

0088 


ANT 


=  0;  "/.l=scale  (both  directions)  l=Fly  in  ANT  center,  0=AFRL 


•/•/.  ramrammmx 

’/,’/,  Configure  Solver  ’/,’/ 

y.mmmmr/.mx 

limits. nodes  =  30;  "/.Global  only,  not  used  for  hp  local  w/  GP0PS  3.3 
WHERE_TO_RUN  =  1;  ’/.Location  of  GPOPS:  1=C  drive,  2=1  drive,  3=L  drive 

WHAT_T0_RUN  =  3;  "/.1=GP0PS  2.4  2=GP0PS  3.2  3=GP0PS  3.3 

FASTMODE  =  1;  ’/.l=Modif ied  GPOPS,  else=normal  GPOPS 

’/,  Subroutine  to  set  path  and  initialize  solver 
conf igure_path_f or_gpops  (WHERE_T0_RUN,  WHAT_T0_RUN) 
current_solution  =  []  ;  "/.init 


•/•/.  mmrarammnmrammnrammxra 


Setup  Options  to  define  flight  shell  ’/,’/ 

Coordinates  are:  (vicon)  [x,y,z(m)  ,psi(rad)]  ’/,"/ 


Vi  ViVViViViViViViViViViViViViVViVViViViViViVVV/Xi 


shell .  dt_path_planner  =  0.2;  ’/,s  Time  per  line  in  shell 

shell .  flight_time  =  180;  ’/,s  Makes  901  lines  to  dealer 


’/,  Start  a/c  around  [-8.84  0  0  0]  (8.84m  is  29  ft,  position  not  critical) 

shell .  f  light_time_to_lm  =  5;  "/.s 

shell .  hold_time_at_lm  =  5;  "/,s 

shell .  start_run_point  =  [-8  0  3  0];  ’/.m 

shell .  flight_time_to_start_run_point  =  10;  ’/,s 

shell  .hold_time_at_start_run_point  =  10;  ’/,s 


’/.post  run  (abort  plan  if  no  hook  engagement) : 
shell .  time_to_descend_to_lm  =  10;  ’/.s 

shell .  hold_time_before_land_at_lm  =  5;  ’/,s 

shell .  flight_time_lm_to_land  =  10;  ’/.s 

shell .  time_to_hold_land_position  =  5;  ’/,s 

’/.Total  slots-landing  mode  slots  =  the  time  slot  to  transition  to  land  mode 
shell . landing_slot=  (shell . flightjtime  -  shell . time_to_descend_to_lm  ... 
-shell .  hold_time_before_land_at_lm  -  shell .  flight_time_lm_to_land  ... 
-shell .  timerto  Jiold_land_position)  /shell  .dt_path_planner; 
last_update_line  =  shell .  landing_slot ;  ’/.init .  Changes  during  land  mode 


’/.hook  engagement  plan: 

shell .  hold_at_approach_point  =  5;  ’/.seconds  to  settle/correct  height 
shell  .perch_speed  =  1/12;  ’/.m/s 

shell .  correction.time  =  5;  ’/.blend  in  vertical  tgt  est.  changes  over  5  sec 
shell .  stop_update_time  =  5;  ’/,s — freeze  the  tgt  updates 


’/.’/.  ViViViViViVLViViViViViViViViViViViViVi 

'Vi  Setup  first  trajectory  planner  run  ’/,’/, 

7.7.  ViViViViViViViViViViViViViViViViViVIXi 


dt.gpops  =  0.9;  ’/.Est .  inclusive  loop  time — planner  calc,  delay,  filter... 
P=[80  0;  0  80];  ’/,m“2  CURRENT  uncertainty  in  target  pos  (not  IC)  ,  Cartesian 
IC.P0  =  P;  ’/.Uncertainty  expected  at  next  initial  planning  point  (Cartesian) 


CONST. Pxx_f  =  0.02;  ’/.Final  Covariance  Element  to  be  met 
CONST. Pzz_f  =  0.02;  ’/.Final  Covariance  Element  to  be  met 


IC .  t0=shell .  f  light_time_to_lm+shell  .hold_time_at_lm+  .  .  . 

shell .  f  light_time_to_start_run_point+shell .  hold_time_at_start_run_point ; 
IC.xO  =  shell .  start_run_point  (1)  ;  ’/,m  vicon  frame,  next  planning  point 

IC.yO  =  shell .  start_run_point  (2)  ;  ’/,m  vicon  frame,  next  planning  point 

IC.zO  =  shell .  start_run_point  (3)  ;  ’/,m  vicon  frame,  next  planning  point 

IC.psiO  =  0; 

IC.xdotO  =  0;  ’/.m/s  Exp  obs  horiz  velocity  at  next  planning  point 

IC.zdotO  =  0;  ’/.m/s  Exp  obs  vert  velocity  at  next  planning  point 
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0089 

0090 

0091 

0092 

0093 

0094 

0095 

0096 

0097 

0098 

0099 

0100 

0101 

0102 

0103 

0104 

0105 

0106 

0107 

0108 

0109 

0110 

0111 

0112 

0113 

0114 

0115 

0116 

0117 

0118 

0119 

0120 

0121 

0122 

0123 

0124 

0125 

0126 

0127 

0128 

0129 

0130 

0131 

0132 

0133 

0134 

0135 

0136 

0137 

0138 

0139 

0140 

0141 

0142 

0143 

0144 

0145 

0146 

0147 

0148 

0149 

0150 

0151 

0152 

0153 

0154 


CONST . x_hat_tgt  =  15;  ’/,m  Inertial  Coords,  target  initial  guess 
CONST. z_hat_tgt  =  5;  ’/,m  Inertial  Coords,  target  initial  guess 


•/.•/.  vmmvhvmvxxmxxvixvmvavmvixumxxxvixumxmxxxxxv/x'ixmx 

’/,’/,  Define  physical  limitations — Room,  Quadrotor,  Bearing  Measurement  Sensor 

•/.•/.  mixnnixnnivixnvMxuuixnuuinvmnumxxmxxnmxnvmu 

limits .perch_offset_x  =  0;  '/,m  Desired  offset  from  wire  at  perch  point 

limits .perch_offset_z  =  -0.4;  '/.m 

limits . x_approach_of f set  =-2;  '/,m  Desired  offset  from  wire  at  app.  point 

limits . z_approach_off set  =  -0.4;  ’/,m 

'/.miss  distance  at  app  point  (smoother,  slower) 
"/.Hard  Deck  for  "run"  portion,  vicon  z  (m) 
’/.Flight  ceiling,  vicon  z  (m) 

’/.Allowable  envelope  vicon  x  (m) 

’/.(cameras  are  intermittent  near  the  wall) 
’/.camera  lower  limit  (rad) 

’/.camera  upper  limit  (rad) 


limits . slush 
limits .min.alt 
limits .max_alt 
limits .min_x 
limits .max_x 


=  0 
=  0.8 
=  5.5 
=  -9 

=  9 


limits .beta_min  =  - (30) *pi/180 ; 
limits .betaunax  =  (40)*pi/180; 


limits . xdotjnax  =  0.5 
limits . zdot_max  =  0.5 
limits . xddot_max=  0.5 
limits . zddot_max=  0.5 


’/.m/s  Horizontal  velocity  limit 
’/.m/s  Vertical  velocity  limit 
’/,m/s~2  Horizontal  acceleration  limit 
’/,m/s~2  Vertical  acceleration  limit 


’/,  Truth  data  for  plots,  measurement  generation 
CONST.  x_tgt  =  8.555;  ’/,m 

CONST.  z_tgt  =  4.02;  ’/.m; 

CONST . dt_meas  =  0.33;  ’/.sec.  Expected  time  step  of  available  measurements 

CONST. R  =  .005;  ’/,rad“2  (std  dev  is  a  little  over  4  deg) 

noise=sqrt(CONST.R)*randn(shell.flight_time/CONST.dt_meas,  1)  ;  ’/.meas  noise 
totaluneasurements  =  0;  ’/,  init 


7.7.  y.y.y.’/.y.y.y.y.y.y.y.y.y.y.’/.y.y.y.y.y.y.y.y.y.y.’/.y.y.y.y.y.’/.y. 

’/,’/,  Setup  Unscented  Kalman  Filter  ’/,’/. 

’/:/.  y.y.y.y.y.y.y.y.y.y.y.y.y.y.y.y.y.y.y.y.y.y.y.y.y.y.y.y.y.y.y.y.’/. 

CONST.  NUKFSTATES  =  2; 


alpha 

beta 

kappa 

lambda 

CONST .  scale_param 
CONST. WOm 
CONST. WOc 
CONST . Wukf 


=  le-3;  ’/,  Sigma  point  spread 

=  2;  ’/,  Prior  knowledge  parameter  (2  opt.  for  Gaussian) 

=  0;  ’/,  Secondary  scaling  parameter 

=  alpha‘2* (CONST. NUKFSTATES  +  kappa)  -  CONST . NUKFSTATES ; 

=  CONST . NUKFSTATES  +  lambda; 

=  lambda/CONST. scale.param; 

=  CONST. WOm  +  (1  -  alpha~2  +  beta); 

=  1/2/CONST,  scale.param; 


7.7.  vavmvixmixxmxxuvmixvix:mvixuvixxxxvixumx'i. 

’/,’/,  Setup  Path  Blending  of  tail  for  new  target  estimates  ’/,’/, 

7.7.  v:/xxxvxxvixvixxxvxxvxxixxixix:avxixxixmxxxvxxvxmxm. 

blend_time  =  5;  ’/.seconds  for  blending  at  the  end  of  the  path 

blend.t  =(0 :  shell .  dt_path_planner  :blend_time)  ’  ;  ’/.time  vec.  for  cos  wave 

blend_wave  =  .  5- .  5*cos  (pi/blend_time*blend_t)  ;  ’/.gives  0  to  1  cos  vector 


7.7.  vaixixTixuvhvauixuvmixm:mvixuvix:mvixum:m 

’/,’/,  Setup  End  of  Path — from  approach  point  to  perch  point  ’/,’/, 

7.7.  nnvixxumxuunixmmxxxnnumxxnvixuvixxm'i. 

avg_speed_land_mode  =  2;  ’/,in/s 

approach_time= (limits .perch.off set_x-limits . x_approach_of f set) /  . . . 

convlength(avg_speed_land_mode ,  ’in’  ,  ’m’)  ; 
app_lines=(0 : 1 :  ceil  (approach_time  /  shell .  dt_path_planner) )’ ; 
app_wavelto0= .  5+ .  5*cos (app_lines/app_lines (end) *pi) ;  ’/,  1  to  0  cos  vector 

dx_f rom_perch=( . 5+limits .  perch.of f set_x-limits . x_approach_of f set)  . . . 
*app_wavelto0;  ’/.correction  splice 
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0155 

0156 
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0181 
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0186 

0187 
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0189 
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0194 
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0198 

0199 

0200 

0201 

0202 

0203 

0204 

0205 

0206 

0207 

0208 

0209 

0210 

0211 

0212 

0213 

0214 

0215 

0216 

0217 

0218 

0219 

0220 


Get  User  inputs — check  IP/subnet  settings,  get  initial  start  point 

y.y.  y.yy/.y.y.y.y//.y.y//.y.yy/.y.yy/.y.y.y.y//.y:/;/.y.yy/:/.yy/.y.y.y.y//.y.yy/.y.yy/.y.y.y.y.y.y.y:/.y.y//.y:/y/.y.y.y.y.y.y:/;/.y.y.y."/.y.y.y.y. 

if  sim==l  "/.for  the  sim,  just  pick  a  hardcoded  spot 
current_pos=get_current_position_sim(0,  [] ) ; 
if  ANT==1 

current  _pos  (2)  =0 ; 

end 

spr intf  (  ’ y.s ’ ,  ’ NOT  ONLINE— SIMULATING  CURRENT  P0S  AND  MEASUREMENT  DATA’) 

else 

conf  irml=’n’  ; 
while  conf irml~=’y ’ 


disp( ’ 


’) 


disp( ’ — ’ ) 

^aVvvavavmvanm:ai:mvhvmmavhvmmavhvmmmv'> 
di  sP  ( >  y.yx/.yx/.y.y.y:m:/.y.yx/.yx/.y.yy/:/.y.y:/.y.yx/.yx/.y.y.y:/.y.y:/.y.yx/.y.y.y.y.yy/:/.y.y;/.y.y.y.  ’  > 
disP('yx/x/x/x/.y.myx/x/x/xmnyx/x/.yx/xmmnyx//mx/.y.mmy.’) 

disp( ’ — ’ ) 


disp( ’ 


’) 


di  sp  ( >  y,y.y.y.y.y.y:/:/:/:/„y.y.y„y:/.y.y.y.y.y.y„y.y:/:/.y:/„y.y.y.y:/.y.y,y:/:/:/„y.y.y„y.y.y.y:/:/.y.  > ) 

disp( "/.System  initialized  in  real-world  flight  mode.’/,"/,’) 

di  sp  ( >  vavivxmvauiv:mvmvhvmmavhvmuv/.vm  • ) 


disp( ’ — ’ ) 
disp( ’ — ’ ) 
if  ANT==1 

disp( ’System  is  in  ANT  Center  mode — flight  arena  scaled’) 
disp( ’ Acknowledge  with  any  key’) 

else 

disp(’ System  is  in  AFRL  mode — no  scaling’) 
disp( ’ Acknowledge  with  any  key’) 

end 

pause 


disp(’Apply  Network  Settings:’) 
disp(’IP:  192.168.10.91’) 
disp(’Subnet  mask:  255.255.255.0’) 

conf irml=input ( ’Update  settings.  When  correct,  enter  ”y”:’,’s’); 

end 

conf  irm2=’n’  ; 
while  conf irm2”  =  ’y ’ 
disp( ’  —  ’  ) 
disp( ’  —  ’  ) 

disp( ’Conf irm  GS/UAV/vicon  on,  check  frame  rates,  reasonable  data’) 

disp(’Input  current  (start  flight)  position,  Vicon  Frame — ’) 

initial jx  =  input  ( ’x(m)  =  ’) ; 

initial_y  =  input ( ’y(m)= ’) ; 

initial_z  =  input ( ’z (m)= ’) ; 

initial_psi_deg  =  input ( ’psi (deg)=’ ) ; 

disp( ’Conf irm  Initial  Position  (case  sensitive)’) 

initial _x 

initial_y 

initial  _z 

initial_psi_deg 

conf  irm2=input  ( ’  If  correct,  enter  ”y”  ,  else  enter  ”n’’:’,’s’); 

end 

"/.set  initial  position  [linejnum  x  y  z  psi  (rad)] 

current_pos= [0  initial_x  initial.y  initial.z  initial_psi_deg*pi/180] ; 

end 


y.y.  yx/x/x/x/x/x/x/x/x/x/x/xm 

Create  first  path  shell  for  dealer 

r/.  y.y.y.y.y.y.y.y.y.y.y.y.y.y.y.y.y.y.y.y.y.y.y.y.y.y.y.y.y.y.y.y.y.% 

if  ANT==1 


current_pos=scale_pos_f  rom_ANT (current.pos)  ; 

end 
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0221 

0222 

0223 

0224 

0225 

0226 

0227 

0228 
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0231 

0232 
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0241 
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0251 
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0256 

0257 
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0260 
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0276 
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0279 

0280 

0281 
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0283 

0284 

0285 

0286 


■/.init : 

data.f  or_dealer_all=zeros (shell . flight  .time/shell . dt_path_planner+l ,4,200) ; 
path  =  build_front_shell_for_dealer(shell,  current.pos) ;  "/.build  path 

Fill  in  a  sample  set  of  run  data 
Start.time  =  cputime; 

current.solution  =  trajectory_planner(IC,  limits ,  current.solution)  ; 

GPOPS.init.time  =  cputime-Start.time  "/.Display  initial  epoch  planning  time 
current.solution.  run_time=GP0PS_init_time ; 

path  =  build_data_f or.dealer (path,  current.solution,  shell); 


7.7.  y.7.7.7.7oy.7.7.7.7.%7.7.7.7.7.y.7.7.7.7.7o%7.7.7.7.y.7.7.7.y.7o%7.7.7.7.7o7.7.7. 

’/,"/,  Set  up  file  saving  for  post  processing  "/,’/, 

%y.  mnmmmfflmnfflfflmmmmn 


data_f  or_dealer_all  ( : 

,  :  , 1)  =  path; 

®/0save  each  path 

gpops 

=  cell(200, 1) ; 

'/.save  GPOPS  solutions 

meas_save=cell (ceil (shell . f light  _time/C0NST, 

.  dt _meas)  ,  1)  ;  ’/.save  measurements 

loop_time_save 

=  zeros (300, 1) ; 

7oinit 

actual-position 

=  zeros (300, 5) ; 

#/0init 

actual_position(l ,  :) 

=  current.pos; 

#/,init 

current .solution . IC 

=  IC; 

x.est 

=  zeros (300, 1) ; 

8/*init 

z.est 

=  zeros (300, 1) ; 

°/«init 

x.est (1) 

=  CONST.  x_hat  _t  gt ; 

z.est (1) 

=  CONST,  z  Jtiat  _tgt ; 

gpops{l} 

=  current .solution; 

P.save 

=  cell (300 , 1) ; 

°/0init 

break.loop 

=  0; 

y.y.  ramrammmranrammnrammnramramra,m 


"/,'/,  Plot  initial  path  for  visual  error  checking  prior  to  takeoff  ’/,"/, 

y.y.  y.y.7.7.y.y.7.7.7.7.y.7.7.7.7.y.y.7.7.7.7.7.y.y.7.7.y.y.7.y.7.y.y.y.7.7.7.y.y.7.y.7.7.y.y.7.7.7.y.7.7.7.7.7.y.y.7.y.7.7.7.y.y. 


zero_to_2pi  =  (0:2:360)  *  (pi/180);  "/.rad  (to  display  covariance  circle) 
’/,2xN,  pairs  of  points  on  unit  circle: 

points_on_unit_circle=  [cos  (zero_to_2pi) ;  sin(zero_to_2pi)  ]  ; 
old=plot_init_96  .  .  .  "/.initialize  main  plot 

(current.solution,  IC,  limits  ,  current.pos  .point s.on.unit.circle)  ; 
if  ANT==1 


scaled_path=scale_path_to_ANT(path) ; 

plot  _dealer_path(scaled_path,  shell ,  0  .figure  .cascade) 

else 


plot  .dealer.path (path, shell , 0 , f igure.cascade) 

end 

disp( ’  —  ’ ) 
disp( ’  —  ’ ) 

disp(’ System  Initialization  Complete’) 

disp( ’Confirm  valid  initial  path,  press  any  key’) 

pause 


y.y.  ramrammmranrammnrammnramramranramm 

"/,'/,  Connect  to  dealer  function,  hold  until  dealer  accepts  initial  path  ’/,"/, 

y.y.  mfflmfflfflmfflnfflnfflfflffliafflffliaffimffimranffiffl); 

if  sim~=l 


"/,  IP  Settings 
PORT.CMDLST  =  49993; 
PORT  .TRUTH  =  49992 ; 


"/,  Info  for  cutting  path  packets 
NUM.C0MMANDS_PER.LIST  =  901; 
NUM.C0MMANDS.PER.PACKET  =  19 ; 

NUM.VALUES.PER.C0MMAND  =  4 ; 
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"/,  Create  a  TCP  Server  to  send  command  lists 
sockconCMDLST  =  pnet  (  ’  tcpsocket  ’  ,P0RT_CMDLST)  ; 
if  sockconCMDLST  ==  -1 

error ( ’Specif ied  TCP  port  unavailable  for  command  lists’); 

end 

disp( ’ Command  List  Server  created’); 

"/,  Create  a  TCP  Server  to  poll  for  truth  and  truth  history 
sockconTRUTH  =  pnet  (  ’tcpsocket  ’  , PORT  TRUTH)  ; 
if  sockconTRUTH  ==  -1 

error ( ’Specif ied  TCP  port  unavailable  for  truth  messages’); 

end 

disp(’Truth  Server  created’); 

disp( ’Waiting  for  connections  ...  Start  the  Dealer  Now’) 

I  Blocks  indefinitely  until  client  connects  for  command  lists 
conCMDLST  =  pnet (sockconCMDLST,  ’tcplisten’ ) ; 
disp( ’ Command  List  Connection  accepted’); 

"/,  Blocks  indefinitely  until  client  connects  for  truth  and  truth  history 
conTRUTH  =  pnet (sockconTRUTH,  ’tcplisten’); 
disp(’Truth  Connection  accepted’); 

disp( ’ Connection  to  Dealer  valid.  Press  "Connect"  in  ground  station’) 

"/,  Wait  for  the  ok  before  sending  command  lists 
okCMDLST  =  pnet (conCMDLST,  ’read’,  1,  ’swap’); 
if  okCMDLST(l)  ~=  ’1’ 

disp(’Error  receiving  ok  to  start  sending  command  lists  ’); 

end 

"/,  Send  path  to  dealer 
if  ANT==1 

scaled_path=scale_path_to_ANT(path) ; 

SendPathList (conCMDLST, scaled_path) ; 

else 

SendPathList (conCMDLST, path) ; 

end 

end 


n  mmrammmy.rararammramm 


0329 

0330 

0331 

0332 

0333 

0334 

0335 

0336 

0337 

0338 

0339 

0340 

0341 

0342 

0343 

0344 

0345 

0346 

0347 

0348 


hit  any  key’) 


Pre-run  loop:  takeoff  to  start_run  point 

dist_from_start_run_point=10;  "/.init  (big) 

dist_f rom_approach_point  =10;  "/.init  (big) 

disp( ’Waiting  to  start  Pre-loop — ’) 
disp(’Turn  on  Data  Recorder,  check  valid  IMU  output, 
pause 

disp(’Start  Ground  station  first,  then  Matlab:’) 
disp(’  1) 

disp(’  2) 

disp(’  3) 

disp(’  4) 

pause 

sprintf ( ’Autopilot  engaged,  preloop — climbing  to  start  run  position’) 


IP  Flight’) 

Motors  on’) 

Start  IP  Command  RX’) 

Start  MATLAB  pre-loop  (hit  any  key)’) 


if  sim==l  "/.For  simulation,  just  enter  the  preloop  for  a  few  seconds 

run_preloop_time  =  3;  "/.seconds 
time_start_preloop  =  cputime; 
time_integer  =  floor (run_preloop_time) ; 

else 


0349  time_integer  =  floor(IC.tO)  ; 

0350  end 

0351 


0352  "/.Run  preloop  until  the  start  run  time 
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0417 

0418 


while  current.pos (1) *shell . dt_path_planner  <=  IC.tO-l 

if  sim==l  "/.for  simulation,  just  exercises  the  preloop 
preloop_time_remaining=run_preloop_time-  (cputime-time_start_preloop) ; 
if  preloop_time_remaining  <=  time.integer 

sprintf('SIM  MODE.  Time  remaining  in  pre-loop='/,l . Of  ’  .time .integer) 
time_integer=time_integer-l ; 
end 

if  preloop_time_remaining  <=0 

'/.set  sim  to  1  second  prior  to  run,  hovering  at  start  run  point 
current_pos=  [ceil ( (IC. t0)/shell . dt_path_planner)  .  .  . 

IC.xO  IC.yO  IC.zO  IC.psiO] ; 

end 

else  '/.get  current  position  from  Dealer 

current_pos  =  get_current_position(conTRUTH)  ; 

’/,  AFRL  vs  ANT  lab  sign  swap — temp  fix  (number  1/3) 
current_pos(4)=-current_pos(4)  ; 

if  ANT==1  "/.scale  if  in  ANT  center 

current_pos=scale_pos_f  rom_ANT (current.pos)  ; 

end 

'/.display  time  remaining  every  integer 
preloop_time_remaining=floor(IC.tO-current_pos(l)  .  .  . 

*shell  ,dt_path_planner)  ; 
if  preloop_time_remaining<time_integer 
time_integer=preloop_time  .remaining; 

sprintf(’FLY  MODE.  Time  to  start  run:  "/,1 .  Of  ’  ,time_integer) 

end 

end 

end 


7,7.  7o7.7.7o7.7.7.7.7.7.7.7.7.7.7o7.7.7o7.7.7.7. 

7,7.  Init  Main  Run  loop  7.7, 

7,7.  7o7o7o7.7o7o7o7o7o7o7o7.7o7o7o7o7o7o7o7.7o7o 

loop_ctr=2;  7,counter  for  knowing  which  slice  to  put  data_f  or  .dealer  into 
sprintf ( ’Begin  Main  Run  Loop5) 

slot-last _meas=ceil  ( (IC .  tO-CONST .  dt  jneas)  / shell .  dt_path_planner) ; 
meas  =  []  ;  7,no  initial  measurements 

approach_point=  [CONST. x Jiat _tgt+limits . x_approach_of f set  .  .  . 

CONST. zJiat.tgt+limits . z_approach_of f set]  ; 
start  _loop_time=cputime ; 

if  sim==l  7.sim  mode:  generate  a  fake  list  of  time  slots  for  measurements 
linejneas_sim=  .  .  . 

ceil  ( 152 :  CONST .  dt  jmeas/ shell .  dt_path_planner :  shell .  landing_slot ) ; 
sim.clock  =  cputime; 

display  .fudge  =  0;  7,  compensation  factors  (sim  only)  for  graphics  time 

display  .time  =  0; 

end 


7,7.  7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7o7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7o7o7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7o7.7. 

7,7.  Main  Run  Loop.  Initiated  from  hover  at  start  run  point  7.7, 

7,7.  7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7o7.7.7.7.7.7.7.7.7.7o7.7.7.7.7o7.7.7.7.7.7.7.7.7.7.7.7.7.7.7. 

while  dist_from_approach_point>l  I  I  P(l,l)>C0NST.Pxx_f  I  I P(2,2)>C0NST.Pzz_f 


7.  Splice  GP0PS  into  shell,  recalc  the  land  mode  to  match  end  point 
[path_to_old_tgt  approach_slot]  .  .  . 

=build_data_f  or_dealer  (path,  current  .solution,  shell); 


7,  Blend  tail  for  changes  in  target  estimate 

path=blend_path_to_updated_tgt (path_to_old_tgt ,  current.pos,  shell,  ... 
blend_time,  blend.wave , approach-slot ,  approach-point); 
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"/,  Save  for  post  processing 

data_f  or_dealer_all  (  :  ,  :  ,loop_ctr)=path; 

"/,  Send  path  to  Dealer 
if  sim~=l 

if  ANT==1 

scaled_path=scale_path_to_ANT(path) ; 

SendPathList (conCMDLST , scaled.path) ; 

else 

SendPathList(conCMDLST,path) ; 

end 

end 

Project  down  path,  calc  the  conditions  and  expected  cov  at 
next  GP0PS  update  time,  bias  for  being  off  of  commanded  position 
pos_error=path(current_pos(l)  ,  :  )  -  current_pos (2 : 5)  ; 

IC=Calcjnext_IC (current.pos,  pos.error,  IC, limits , current.solution,  .. 
P,  shell . dt_path_planner , dt_gpops ,  slot.last.meas); 

Plan  next  optimal  path 
St  art  .time  =  cputime; 

current.solution  .  .  . 

=traj  ectory  .planner  (IC ,  limits ,  current  .solution)  ; 
current_solution.run.time  =  cputime-Start.time ; 
loop_time_save(loop_ctr)  =  cputime-start.loop.time ; 
start.loop.time  =  cputime; 

7,7,  Get  position,  time 
if  sim==l 

display _fudge=display_fudge+display .time ;  '/,sim:  add  graphics  time 
°/,obtain  position  from  ground  station  [linejnum  x  y  z  psi]  : 
current_pos=get_current_position_sim(f  loor  ( (29+cputime  .  .  . 
-sim.clock-display  .fudge) /shell .  dt.path.planner) .path) ; 

else 

"/.obtain  position  from  ground  station  [line_num  x  y  z  psi] 
current_pos=get_current_position(conTRUTH) ; 

’/,  AFRL  vs  ANT  lab  sign  swap — temp  fix  (number  2/3) 
current_pos(4)=-current_pos(4)  ; 

’/,  Scale  back  up  if  in  ANT  Center 
if  ANT==1 

current_pos=scale_pos_f  rom_ANT (current.pos)  ; 

end 

end 


"/,  Run  the  display,  if  on 
if  display _on==l 

start .display  _time=cputime ; 

old=plot_single_display96(current_solution,  IC, limits, old,  ... 

current.pos  .path, meas , point s.on.unit .circle) ; 
display _time=cputime-start_display .time ;  7, time  spent  making  display 

end 


"/,"/,  Record  Data  for  post  processing 


actual_position(loop_ctr . 
current.solution . IC 
gpopsjloop.ctr} 
x.est (loop.ctr , 1) 
z.est (loop.ctr , 1) 


:)  =  current.pos; 

=  IC; 

=  current.solution; 
=  CONST .  x_hat_tgt ; 

=  CONST .  z_hat_tgt ; 


"/,’/,  Get  measurement  locations  from  Ground  station 
if  sim==l 
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7,12x5  [line_num  x  y  z  psi]  (last  12  meas  locations,  not  in  order) 
measTLocat ions=get_meas_locat ions _sim (path, current _pos , line_meas_sim) ; 
else 

meas_locations=get_meas_locations  (conTRUTH)  ; 
if  ANT==1 

’/,scale  up  the  meas  locations 

meas_locations=scale_pos_f rom_ANT(meas_locations) ; 
end 
end 

7,7,  Sort  measurements  that  have  not  been  incorporated  (may  be  empty) 

[meas  slot_last_meas]  =  get_beta  (meas_locations ,  slot_last_meas) ; 

7,7,  If  there  are  new  measurements,  generate  new  target  estimate  and  cov 

7.7,  matrix  with  Unscented  Kalman  Filter 
new_meas=length (meas ( : , 1 ) ) ; 

if  newjneas  >  0 

7»add  measurement  noise 
meas(:,3)=  meas(:,3)  ... 

+  noise  (total_measurements+l : total jneasurements  +  newjneas) ; 
total_measurements=total_measurements  +  new  meas  ; 
for  i=l :new_meas  7.UKF 

[Xrel  P] =UKF -Cartesian (meas (i , : ) ,P) ; 

CONST. x_hat_tgt=Xrel(l)+meas (i ,  1) ;  7,update  tgt  estimate 
CONST .  z_hat_tgt=Xrel (2)  +meas  (i ,  2)  ; 

end 

end 

7.7.  Save  for  post  processing 
meas_save{loop_ctr}=meas ; 

7.7.  Update  approach  point  with  new  tgt  estimate 
approach_point= [CONST .  x_hat_tgt+limits . x_approach_off set  . . . 

CONST. z_hat_tgt+limits . z_approach_of f set] ; 
dist_f  rom_approach_point  =  norm(approach_point  -  current_pos  (  [2  4])); 

loop_ctr=loop_ctr+l ; 
if  display _on==l 

sprintf  ( ’gpops :  7.g  sec,  loop:  7.g  sec,  dx=  7.g,  dz=  7.g’ ,  current_solution.run_time,loop_time_save(loop_ctr) ,  ... 
CONST .  x_tgt-C0NST .  x_hat_tgt ,  CONST .  z_tgt-C0NST .  z_hat_tgt) 

end 

end  7.  end  main  loop 


sprintf ( ’Exiting  Main  Loop — Required  Position  &  Covariance  Achieved’) 
Tgt_est_error_at_approach_point_inches=convlength(  [CONST. x_tgt .  .  . 

-CONST. x_hat_tgt  CONST. z_tgt-C0NST.z_hat_tgt]  , ’m’  ,  ’in’) 
y.update  display  when  achieving  approach  position 
if  sim==l  &&  display _on==l 

old=plot_single_display96(current_solution,  IC, limits,  ... 

old,  current _pos  .path, meas  .point s_on_unit_circle) ; 
titleA=sprintf  ( ’Approach  Parameters  Achieved — True  Tgt  Err:  x=7,1.3g(in)  z=7,l  .3g(in)  ’  ,  ... 

Tgt_est_error_at_approach_point_inches  (1) ,  Tgt_est_error_at_approach_point_inches  (2) ) ; 
title(titleA, ’fontsize’ , 14) 

end 


7.7.  7.y.7.7.7.7.y.7.7.y.7.7.y.7.7.y.7.7.7.7.y.7.7.7.7.7.y.7.7.y.7.7.7.7.y.7.7.y.7.7.y.7.7.7.7.7.7.7.y.7.7.y.7.7.y. 

7.7.  Initialize  Landing  Mode,  splice  approach  into  shell  7.7. 
7.7.  7.7.7.7.7.7.7.7.7.7.7.7.y.7.7.7.7.7.7.7.7.7.7.y.7.7.7.7.7.y.7.7.7.7.y.7.7.7.7.7.y.7.7.7.7.7.7.7.7.7.7.y.7.7.7. 

row2=approach_slot ; 


7.  X  perching  profile 
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x_remaining=limits  .perch_off  set_x-path(approach_slot , 1) ; 

’/.use  the  wave  to  smooth  in,  starting  backwards  from  tgt  and  using  only  the 
’/.distance  that  you  have  left 

path_x=C0NST.  x_hat_tgt+limits  .perch_of  f  set_x-dx_f  rom_perch .  .  . 

(dx_f rom_perch<x_remaining)  ; 
row3=row2+length(path_x)  ;  ’/.find  slot  to  splice  into 
path(row2+l :row3, l)=path_x;  '/.splice  in  x  approach  profile 
path(row3+l : end, l)=path(row3, 1) ;  ’/.hold  final  x  for  the  rest  of  the  flight 

7,  Z  perching  profile 

z_perch=C0NST.z  Jiatvtgt+limits  .  perch_of f set_z; 

"/.hold  and  additional  2  seconds  past  when  x  finishes 
row3_plus2sec=row3+ceil  (2/shell .  dt_path_planner )  ; 
path(row2+l :  row3_plus2sec  ,3)  =z_perch; 

dist_per_line_lin_per_sec=convlength(l ,  ’in’  , ’m’ )  *shell .  dt_path_planner ; 

"/,  Drop  5  inches  (.127m)  at  1  in  per  sec,  then  lm  at  2in  per  sec 
append3_z=  [z_perch: -dist_per_line_lin_per_sec :  z_perch- .  127,  .  .  . 

z_perch- .  127 :  -2*dist_per_line_lin_per_sec : z_perch-l]  ’ ; 
row_complete=row3_plus2sec+length(append3_z) ; 
path(row3_plus2sec+l :  row.complete  ,3)=append3_z ; 

if  row_complete<shell . landing_slot 

7,  If  no  wire  engagement,  hold  the  last  z  position  until  landing  time 
path(row_complete+l : shell . landing_slot ,3) =path(row_complete ,3) ; 

end 

7.  Recalc  z  from  end  of  run  point  to  1  m  hover  (the  rest  doesn’t  change) 
row_lm=shell .  landing_slot+shell . time_to_descend_to_lm  .  .  . 

/shell. dt_path_planner; 
path(shell .  landing_slot+l :  row_lm,  3)  .  .  . 

=linspace  (path (shell .  landing.slot  ,3)  ,  1  ,row_lm-shell .  landing_slot)  ; 

last_z_update=l ;  '/.init,  just  for  recording  the  last  updated  value 


7.7.  Send  path 
if  sim~=l 

if  ANT==1 

scaled_path=scale_path_to_ANT(path) ; 
SendPathList  (conCMDLST,  scaled_path)  ; 

else 

SendPathList (conCMDLST, path) ; 

end 

end 


7.7.  Save  for  post  process 
data.f  or_dealer_all  ( :  , :  ,loop_ctr)  = 
actual_position(loop_ctr , :) 
P_save{loop_ctr} 

loop_time_save (loop.ctr)  =  cputime 

start_loop_time 

x_est (loop.ctr , 1) 

z_est (loop_ctr , 1) 

perch_loop_ctr 

loop_ctr 


=  path;  ’/.save  path 

=  current_pos;  '/.save  actual  position 

=  P;  '/.save  est  covariance 

-start_loop_time ;  '/.save  loop  process  time 
=  cputime;  '/.restart  loop  time 

=  CONST. x_hat_tgt; ’/.save  current  tgt  est 
=  CONST. zJiatvtgt; ’/.save  current  tgt  est 
=  loop.ctr ; ’/.identify  when  main  loop  ended 
=  loop_ctr+l;  '/.increment  loop  ctr 


7.7.  7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.’/.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7. 


7.7.  Perch  loop:  Check  position,  get  measurments  (only  count  those  in  F0V  7.7. 
7.7.  limits).  Update  tgt  if  valid  measurements.  Correct  path  if  valid  7.7. 
7.7.  tgt  update.  Keep  loop  going  until  5  seconds  after  the  system  should  7.7. 
7.7.  have  perched.  7.7. 


7.7.  7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.'/.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7. 


sprintf ( ’Entered  Perching  Mode  Loop’) 
while  current.pos  (1)  <  row_complete 
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’/,’/,  Get  current  position  and  measurement  locations 

if  sim==l  "/simulate  position  from  ground  station  [line_num  x  y  z  psi] 
current_pos=get_current_position_sim(f  loor  (  (29+cputime  .  .  . 

-sim_clock-display .fudge) /shell . dt_path_planner)  .path) ; 

'/012x5  [line_num  x  y  z  psi]  (last  12  meas  locations,  not  in  order) 
meas.locat ions=get_meas_locat ions _sim (path, current.pos , line .meas.sim)  ; 
else  ’/.obtain  position  from  ground  station  [linejium  x  y  z  psi] 

current_pos=get_current_position(conTRUTH)  ; 
meas_locations=get _meas_locations (conTRUTH) ; 

’/,  AFRL  vs  ANT  lab  sign  swap — temp  fix  (number  3/3) 
current_pos(4)=-current_pos(4)  ; 

if  ANT==1 

’/.scale  up  the  meas  locations 

meas_locations=scale_pos_f rom_ANT (meas .locations) ; 

’/.scale  up  current  position 
current_pos=scale_pos_f  rom_ANT (current.pos)  ; 

end 

end 


’/,’/,  Check  for  valid  measurments  (discard  those  outside  of  true  F0V) 
meas_locations=discard_meas_outside_FOV  . . . 

(meas .locations .limits . bet  a_min, limits .beta_max) ; 

’/,’/,  Sort  measurements  that  have  not  been  incorporated  (may  be  empty) 
[meas  slot.last.meas]  =  get.beta  (meas.locations ,  slot.last.meas); 

’/,’/,  If  there  are  new  measurements,  update  target  estimate  and  cov 
’/,’/,  matrix  with  Unscented  Kalman  Filter,  then  update  the  path 
if  "isempty (meas) 

new _meas=length (meas ( : , 1) ) ; 
meas(:,3)  =  meas(:,3)  ... 

+  noise  (total  _measurements+l :  total  .measurements  +  newjneas)  ; 
total  _measurements=total_measurements  +  new_meas; 
for  i=l:new_meas  ’/.UKF 

[Xrel  P] =UKF .Cartesian (meas (i , : ) ,P) ; 

CONST . x Jiat_tgt=Xrel(l)+meas (i ,  1) ;  ’/.update  tgt  estimate 
CONST . z_hat_tgt=Xrel (2)  +meas (i , 2)  ; 

end 


’/."/.  Update  the  path 

’/,’/,  (if  past  hold  at  approach  point  and  more  than  4  in  from  perch) 
’/.once  past  the  hold,  resume  updating  path 

if  current.pos (1)  >  row2  &&  current_pos(l)<last_update_line 
’/.Update  x: 

x_remaining=CONST.x_hat_tgt+limits.perch_off set_x-path  .  .  . 

(current.pos  (1) ,  1) ;  ’/.dist  in  x  still  to  go 
’/.(don’t  go  from  actual  position,  else  you’ll  correct  errors 
’/.for  the  integrator  and  never  allow  it  to  zero  out) . 
path_x=C0NST.  xjiat.tgt+limits  .  perch.of  f  set_x-dx_f  rom.perch  .  .  . 
(dx_f  rom_perch<xjremaining)  ; 

’/.append  to  path 

rowPer ch=current_pos  ( 1 )  +length  (path_x)  ; 
path  (current.pos  (1)  +  1 :  rowPerch,  l)=path_x; 

’/.hold  that  x  for  the  rest  of  the  flight 
path (r owPerch+1 : end , 1 ) =path (rowPer ch , 1 ) ; 

’/.Update  z  (move  at  fixed  velocity  to  correct  error) 
z_perch=C0NST .  z_hat_tgt+limits  .perch.of  f  set _z ; 
if  path(current_pos(l)  ,3)  <=  z.perch  ’/.if  cmd  is  low,  move  up 
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append4_z=  [path(current_pos(l)  ,3)  .  .  . 
+dist_per_line_lin_per_sec : dist_per_line_lin_per_sec  . . . 
:z_perch,  z.perch] 1 ; 

else  "/if  current  cmd  is  high,  move  down 
append4_z=  [path(current_pos(l)  ,3)  .  .  . 
-dist_per_line_lin_per_sec  .  .  . 
:-dist_per_line_lin_per_sec:z_perch,  z.perch]  ’ ; 

end 

"/.stop  sending  new  paths  at  4  in  (should  be  out  of  F0V  anyway) 
if  current_pos(l)+append4_z  >  last_update_line  "/.freeze  path 
append4_z=append4_z(l :  last_update_line-current_pos(l)  )  ; 

end 

row4z=current_pos  ( 1 )  tlength (append4_z)  ; 
path ( current  _pos  (1)  +  1 :  row4z  ,  3)=append4_z ; 

"/.should  have  frozen  at  4in,  so  the  if  is  redundant — hold  until 
1,2  sec  after  x  reaches  perch 
if  row4z<rowPerch 

rows2sec=ceil (2/ shell .  dt_path_planner) ; 

path (row4z+l : rowPerch+rows2sec , 3) =path(row4z , 3) ; 

end 

'/,  Drop  5  inches  (.127m)  at  1  in  per  sec,  then  lm  at  2in  per  sec 
append5_z=  [path(row4z,3)  : -dist_per_line_lin_per_sec  .  .  . 
:path(row4z ,3)- . 127,  path(row4z ,3) - . 127 : -2  ... 
*dist_per_line_lin_per_sec :path(row4z , 3)  —  1]  ’ ; 
row_complete=rowPerch+rows2sec+length(append5_z)  ; 
path(rowPerch+rows2sec+l :  row.complete  ,3)=append5_z; 

if  row.complete  <  shell . landing.slot 

"/,  Hold  the  last  z  position  until  landing  mode 
path (row_complete+l : shell . landing.slot ,3)  ... 
=path(row_complete , 3) ; 

end 

"/,  Recalc  z  from  end  of  run  to  lm  hover  (rest  doesn’t  change) 
path(shell .  landing_slot+l :  row.lm,  3)=linspace  .  .  . 

(path(shell .  landing.slot  ,3)  ,  1  ,row_lm-shell .  landing.slot)  ; 

"/.make  last  update  line  happen  4  in  from  the  perch 
x_perch_minus_4in  =  CONST .  x_hat_tgt+limits  .perch_of  f  set_x- .  1 ; 
slots_past4in  =  f  ind(path(  :  ,  1)  >x_perch_minus_4in)  ; 
last_update_line  =  slots_past4in(l) ; 


'/,  Send  path 
if  sim~=l 

if  ANT==1 

scaled_path=scale_path_to_ANT(path) ; 
SendPathList  (conCMDLST ,  scaled.path)  ; 

else 

SendPathList(conCMDLST,path) ; 

end 

end 

end 


end 


"/,’/,  Save  for  post  process 
data_f  or  .dealer  _all  (  :  ,  :  ,loop_ctr) 
actual_position(loop_ctr , :) 
loop_time_save(loop_ctr) 
start_loop_time 


=  path; 

=  current  pos ; 

=  cputime-start_loop_time ; 
=  cputime; 
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0749  x.est (loop.ctr , 1) 

0750  z_est (loop_ctr , 1) 


=  CONST.  x_hat  _tgt ; 
=  CONST,  z  Jiat  _tgt ; 
=  meas ; 

=  P; 


0751  meas_save{loop_ctr} 


0752  P_save{loop_ctr} 
0753 


0754  loop.ctr 


=  loop_ctr+l; 


0755 

0756  end  7»end  perch  mode  loop 
0757 

0758  x_hat_at .freeze  =  CONST. x Jiat _t gt ; 

0759  zJiat.at .freeze  =  CONST. z Jiat _t gt ; 

0760 

0761  sprintf(’No  more  path  updates  being  sent’) 

0762 

0763  Final_Tgt_est_error_inches=convlength  .  .  . 

0764  (  [CONST.  x.tgt-CONST .  x  Jiat  _tgt  CONST .  z.tgt-CONST .  zJiat.tgt]  , ’m’,’in’) 

0765 

0766  save  last_run  '/.save  workspace 
0767 


0768  •///,  mramnmmmnm 

0769  7.7.  CLOSE  THE  CONNECTIONS  7.7. 

0770  7.7.  7.7o7.7.7.7.7.7.7.7.7o7.7o7o7o7.7.7.7.7.7.7.7.7.% 

0771  if  sim~=l 

0772  RequestConnectionClose(conTRUTH) ; 

0773 

0774  7.  Close  the  connection  and  socket 

0775  pnet (conCMDLST,  ’  close’); 

0776  pnet (sockconCMDLST,  ’close’); 

0777  disp( ’TCP/IP  Command  Connection  closed’); 
0778 

0779  7.  Close  the  connection  and  socket 

0780  pnet (conTRUTH,  ’close’); 

0781  pnet (sockconTRUTH,  ’close’); 

0782  disp( ’TCP/IP  TRUTH  Connection  closed’); 

0783  end 


B.2  Trajectory  Planner  QVOVS  Interface 

The  optimal  solver  calling  function  is  included  to  provide  an  example  of  a  QVOVS 
interface  that  is  set  up  to  run  recursively,  for  real-time  control  applications.  It  provides 
an  example  of  how  to  trim  and  bootstrap  a  previous  guess,  and  it  highlights  the 
different  inputs  required  for  use  with  QVOVS  2.4,  3.2,  and  3.3.  In  particular,  the 
order  and  size  of  the  outputs  change  between  different  versions  of  the  software,  but 
this  is  not  addressed  in  any  of  the  current  documentation.  This  can  cause  significant 
errors,  particularly  with  analytic  derivatives  in  relation  to  the  cost,  DAE,  and  event 
functions.  These  are  provided  with  correct  output  examples  for  all  cases. 
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Trajectory  Planner  QVOVS  Interface  Code. 
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vi  vavmvvinviviiinvmnvi.vavmv/xmv.m.mxmnvavhvmvixi.vvi:/:i. 

7.7.  Trajectory  planner  7#7# 

7.7.  7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7.7. 


7.  This  mfile  sets  up  the  optimal  control  problem  for  the  GP0PS  software 
7#  Written  By:  LtCol  Steven  Ross,  AFIT/ENY  2010. 

7. 


7.  Inputs  are  initial  conditions  (the  projected  position  when  solution  is 
7.  expected  to  become  available),  limits  (room  boundaries,  etc.),  the  last 
7#  solution  (the  "already  flown"  portion  is  cut  off  and  the  remainder  is 
7.  used  as  the  guess),  output  is  output . solution  from  GP0PS.  If  the  solution 
7.  does  not  converge,  the  old  solution  is  sent  back  out  (so  the  next  next 
7.  round  will  not  use  the  new  (bad)  solution  for  the  guess) ,  and  an  error 
7.  message  is  recorded  and  displayed.  x_hat_tgt,  z_hat_tgt  is  the  current 
7.  target  location  estimate 


function  current  .solution  =  trajectory  .planner  (IC,  limits,  Last.Solution) 


global  CONST  FASTMODE  WHAT_T0_RUN 
7.7.  Setup,  Define  Final  approach  point 

7.  check  limits  (if  a  really  bad  estimate  has  put  it  out  of  ceiling/floor 

7.  limits,  put  it  on  the  limit) 

xf =C0NST .  x_hat_tgt+limits .  x.approach.of  f  set ; 

zf  =min (max (CONST.  z_hat_tgt+limits  . z.approach.of  f  set ,  limits  .min.alt)  ,  ... 
limits  .max.alt)  ; 


FIM0=pinv(IC.P0)  ;  7.1nitial  Fisher  Information  Matrix 

straight_time=(xf-IC.xO) /limits  .xdotjnax;  7.min  possible  time 

7.7.  Bounds  on  initial  and  terminal  values  of  time 

limits  .time .min  =  [IC.tO  IC.tO+straight.time]  ;  7.[t0  min  tf  min] 

limits  .time .max  =  [IC.tO  IC.t0+max(10,3*straight_time)]  ;  7.[t0  max  tf  max] 
7#  use  suitable  buffer  of  time,  NLT  10  seconds  if  close  to  target 

7.7.  State  Bounds 

7.x  (Using  "wall"  at  approach  point)  : 

limits . state .min(l ,: )  =  [IC.xO  IC.xO-2  xf-limits . slush] ; 
limits . state .max(l ,: )  =  [IC.xO  xf  xf  ]; 

7.  z 

limits . state .min(2 , :)  . . . textcolorcomment 

=[IC.z0  limits .min.alt  max (limits .min.alt ,zf -limits . slush)] ; 
limits . state .max (2 ,: )  ... 

=  [IC.z0  limits  .max.alt  min(limits .max.alt ,zf+limits . slush)] ; 


7.  x.dot 

limits . state .min(3, : )  =  [IC.xdotO  -limits . xdotjnax  0]  ; 
limits . state .max (3, : )  =  [IC.xdotO  limits . xdotjnax  0]  ; 


7.  z.dot 

limits . state .min(4, : )  =  [IC.zdotO  -limits .zdotjnax  0]  ; 
limits . state .max (4, : )  =  [IC.zdotO  limits . zdotjnax  0]  ; 


7.zetal 

limits . state .min(5 , : ) 
limits . state .max (5 , : ) 


[FIM0(1 , 1)  -100  0]; 

[FIM0(1 , 1)  10000  10000]; 


7.zeta2 

limits .state .min (6 , : ) 
limits . state .max (6, : ) 


[FIM0(2,2)  -100  0]; 

[FIM0(2,2)  10000  10000]; 
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"/,zeta3 

limits . state .min(7 , : ) 
limits . state .max (7 , : ) 


[FIM0(1,2)  -10000  -10000]; 

[FIM0(1 ,2)  10000  10000]; 


Control  Bounds 
limits . control .min 
limits . control .max 


=  [-limits  .xddot_max;  -limits  .  zddotjnax]  ; 
=  [  limits  .xddot_max;  limits  .  zddotunax]  ; 


°/0 Bounds  on  an  unknown  static  parameter 
limits .parameter .min  =  [] ; 
limits  .parameter  .max  =  []  ; 


Path  Limits  (maintain  F0V) 
limits .path. min  =  limits  ,beta_min; 

limits .path. max  =  limits ,beta_max; 


7,7,  Event  Constraints  (any  positive  num  indicates  final  covariance  is  met) 
limits . event .min  =  [  0;  0]; 

limits . event .max  =  [le6;  le6] ; 


7.7.  Initial  Guess==>bootstrap  if  Last-Solution  is  provided 
test  =  isf ield(Last_Solution, ’time’) ; 

if  test  7.see  if  the  Last-Solution  exists  (won’t  if  deleted,  or  1st  run) 
7.crop  out  any  parts  of  the  guess  that  will  have  been  flown  past 
index=f  ind(Last_Solution.time>IC.t0) ;  7.get  index  of  future  slots 
if  "isempty (index)  7.if  there  are  future  points,  use  as  guess 
guess. time  =  [IC.tO;  Last-Solution. time(index)] ; 

guess. state  =  [IC.xO  IC.zO  IC.xdotO  IC.zdotO  FIM0(1,1)  ... 

FIM0(2,2)  FIM0(1,2);  Last-Solution. state(index, :)] ; 


guess . control 

= 

[0  0;  Last 

.Solution . control (index, : )] ; 

guess . parameter 

= 

□  ; 

else  °/«May  not  be  future  points  (i.e 

end  of  path,  final  cov  not  met) 

guess .time 

= 

[IC.tO; 

IC.tO+l+straight-time]  ; 

guess . state ( : , 1) 

= 

[IC.xO; 

xf]  ; 

guess . state ( : ,2) 

= 

[IC.zO; 

zf]  ; 

guess . state ( : ,3) 

= 

[IC.xdotO; 

0]  ; 

guess . state ( : ,4) 

= 

[IC . zdotO ; 

0]  ; 

guess . state ( : ,5) 

= 

[FIM0(1,1) 

200]  ; 

guess . state ( : ,6) 

= 

[FIM0  (2 , 2) 

200]  ; 

guess . state ( : ,7) 

= 

[FIM0  (1 , 2) 

200]  ; 

guess . control ( : , 1) 

= 

[0; 

o]  ; 

guess . control ( : , 2) 

= 

[0; 

0]  ; 

guess . parameter 

end 

[ 

]; 

guess .time 

= 

[IC.tO; 

IC.tO+straight_time]  ; 

guess . state ( : , 1) 

= 

[IC.xO; 

xf]; 

guess . state ( : ,2) 

= 

[IC.zO; 

zf]  ; 

guess . state ( : ,3) 

= 

[IC.xdotO; 

0]  ; 

guess . state ( : ,4) 

= 

[IC . zdotO ; 

0]  ; 

guess . state ( : ,5) 

= 

[FIM0(1,D 

200]  ; 

guess . state ( : ,6) 

= 

[FIM0  (2 , 2) 

200]  ; 

guess . state ( : ,7) 

= 

[FIM0  (1 , 2) 

200]  ; 

guess . control ( : ,  1) 

= 

[limits . xdot_max;  -limits . xdot_max] ; 

guess . control ( : , 2) 

= 

[0; 

0]  ; 

guess . parameter 

= 

[ 

]; 

end 


7.  Setup  part  of  the  problem 


setup . name 
setup . f uncs . cost 
setup . f uncs . dae 
setup . f uncs . event 
setup . f uncs . link 


mf ilename; 

’trajectory_planner_cost’ ; 
’trajectory_planner_dae’ ; 
’trajectory_planner_event  ’ ; 
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setup . limits 
setup . guess 
setup . linkages 
setup . direction 
setup . autoscale 
setup . derivatives 
setup . checkDerivatives 
setup .maxlterations 


=  limits; 

=  guess; 

=  []; 

=  ’increasing5;  °/0of  independent  variable 
=  5  on 5 ; 

=  5  analytic 5 ; 

=  0; 

=  500; 


if  WHAT_T0_RUN==2  "/Additional 
"/required  inputs: 
setup.mesh.grid=5hp5 ; 
setup . mesh . nodesbottom=2 ; 
setup . mesh . on= 5  yes 5 ; 
setup . method= 5  radau 5 ; 
setup . solver= 5  snopt 5 ; 
setup . limits . intervals=3 ; 
setup . limits . nodesperint=5 ; 


Options  for  GP0PS  3.2 

*/«5hp5  /  ’global5 

"/fewest  number  of  nodes  to  use 

"/(’yes’  /  ’no’) 

*/  ’  radau ’ ,  ’ gauss  ’ ,  ’  lobatto  ’ 
"Zipopt  not  working  yet 


"/Optional  inputs: 

"/setup . meshdisplay=  ’ yes  ’ ; 
"/setup .  mesh .  tolerance ; 
“/setup  .mesh,  iteration; 
"/setup .  mesh .  guess ; 

"/setup .  controlinterp ; 
"/setup .  mesh .  nodestop ; 
"/setup .  mesh .  splitmult ; 
"/setup . mesh . warm=  ’yes’ ; "/ 


OPTIONAL  (Default 
OPTIONAL  (Default 
OPTIONAL  (Default 
OPTIONAL  (Default 
OPTIONAL  (Default 
OPTIONAL  (Default 
OPTIONAL  (Default 


le-3) 

20) 

’yes ’ ) 

’lagrange’ ) 

setup . nodesbottom+5) 

2) 

’no’ ) 


elseif  WHAT_T0_RUN==3  "/Additional 
setup . mesh . on= ’ yes ’ ; 
setup. mesh. grid=’hp’ ; 
setup . mesh . tolerance=le-3 ; 
setup. mesh. iteration=2; 
setup . mesh . guess= ’yes’; 
setup . controlinterp= ’ lagrange 
setup . mesh . nodesbottom=2 ; 
setup . mesh . nodestop=12 ; 
setup . method= ’ radau ’ ; 
setup . solver= ’ snopt ’ ; 
setup . limits . intervals=3 ; 
setup . limits . nodesperint=5 ; 
setup . mesh . warm= ’ no ’ ; 

"/setup .  mesh .  splitmult=2 ; 


Options  to  run  GP0PS  3.3 
"/(’yes’  /  ’no’) 

"/’local’ ,  ’hp’ ,  ’global’ 
"/.OPTIONAL  (Default  =  le-3) 


’ ;  7, ’  lagrange  ’ ,  ’  linear  ’ ,  ’  cubic  ’ ,  ’  spline  ’ 
"/.fewest  number  of  segment  nodes 
"/.OPTIONAL  generally  should  be  bottom  +  10 
*/#  ’  radau  ’ ,  ’  gauss  ’ ,  ’  lobatto  ’ 

"Zipopt  not  working  yet 


"/.OPTIONAL  (Default  =  ’no’) 
"/.OPTIONAL  (Default  =  2) 


end 


"/.Call  main  function 

if  FASTMODE  ==1 

setup . f  astmode=l ; 

output  =  Ross.gpops (setup) ; 

else 

output  =  gpops (setup) ; 

end 


"/.Use  my  modified  GPOPS  (Ross.gpops) 
"/.don’t  add  this  if  using  normal  GPOPS 


"/.use  standard  GPOPS 


if  output . SNOPT.inf o  ==  1 

current _solution=output .  solution; 

else 

sprintf ( ’ *******DID  NOT  CONVERGE,  FORWARDING  PREVIOUS  SOLUTION*******’) 
current_solution=Last_Solution; 

end 

current .solution . SNOPT.inf o=output . SNOPT.inf o ; 
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Trajectory  Planner  Cost  Function. 


0001  function  [Mayer , Lagrange ,DerivMayer ,DerivLagrange]  ... 

0002  =trajectory_planner_cost (solcost) 

0003 

0004  7.  This  function  works  with  the  optimal  path  solver,  and  provides  the  cost 

0005  /  and  all  of  the  partial  derivatives  when  provided  with  the  path 

0006 

0007  tf  =  solcost .terminal. time; 

0008  U  =  solcost . control ; 

0009  /  tO  =  solcost . initial .time ; 

0010  7*  X0  =  solcost .  initial .  state ; 

0011  7.  Xf  =  solcost  .terminal,  state; 

0012  7o  t  =  solcost  .time; 

0013  7o  X  =  solcost .  state ; 

0014  7.  p  =  solcost  .parameter ; 

0015  7.  iphase  =  solcost  .phase 
0016 

0017  Mayer  =  tf ;  7«  min  final  time 

0018  w  =  .1;  7o  Slightly  weight  control  to  avoid  singular  arc 

0019  Lagrange  =  w*(U( : , 1) . ~2+U( : ,2) . "2) ; 

0020 

0021  7*  Analytic  Derivatives: 

0022 

0023 

0024  if  nargout  ==  4  7*  Can  be  used  for  analytic  derivatives  an  another  option 
0025  [N  ,  m]=size(U); 

0026  DerivMayer=  [zeros (1 , 15)  1]  ;7»[dphi/dX(t0)  dphi/dt_0  dphi/dX(tf)  dphi/dt_f] 
0027  dL_dX=zeros (N , 7) ; 

0028  dL_dU=2*w*U; 

0029  dL_dt=zeros (N, 1) ; 

0030  DerivLagrange=  [dL_dX  dL_dU  dL_dt]  ; 

0031  else 

0032  DerivMayer=  [] ; 

0033  DerivLagrange= [] ; 

0034  end 


Trajectory  Planner  Differential  Algebraic  Equations  Function. 


0001  function  [outputl  output2  output3]  =trajectory_planner_dae (soldae) 

0002 

0003  7.  This  mfile  provides  the  differential  algebraic  equations  for  the 
0004  7.  trajectory  planner,  and  provides  all  of  the  partial  derivatives  when 
0005  7»  provided  with  the  path.  A  path  constraint  is  added  to  keep  the  UAV 
0006  7.  within  camera  F0V  limits.  Outputs  are  different  based  on  which  version  of 
0007  7o  GP0PS  is  being  run,  and  whether  or  not  the  analytic  derivatives  are  being 
0008  7o  used. 

0009  7. 

0010  7.  Output  Formatting: 

0011  7. 

0012  7o  gpops  2.4,  auto  derivs : 

0013  7o  outputl=  [xdot  path];  output2=[]  ;  output3=[]; 

0014  7o  gpops  2.4,  analytic  derivs: 

0015  7o  outputl=  [xdot  path];  output2=  [deriv.dae]  ;  output3=[]; 

0016  7o  gpops  3.~,  auto  derivs: 

0017  7.  output  1=  [xdot]  ;  output 2=  [path]  ;  output3=[]; 

0018  7.  gpops  3.",  analytic  derivs: 

0019  7o  output  1=  [xdot]  ;  output 2=  [path]  ;  output3=  [deriv.dae]  ; 

0020 
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0040 
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0044 

0045 
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0050 
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0055 
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0059 

0060 
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0065 
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0070 
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global  CONST  WHAT.TO.RUN 

X  =  soldae . state ; 

U  =  soldae . control ; 

"/,  p  =  soldae. parameter; 
’/,  t  =  soldae. time; 

’/,  iphase  =  soldae. phase 


rx 

=  CONST. xJiat_tgt-X(:  ,1)  ; 

“/relative  x 

rz 

=  CONST. zJiat_tgt-X(:  ,2)  ; 

“/relative  z 

xdot 

II 

X 

CO 

“/velocity  x 

zdot 

=  X( : ,4) ; 

“/velocity  z 

xddot 

=  U( : ,1) ; 

“/acceleration  x 

zddot 

=  U( : ,2) ; 

“/acceleration  z 

r2 

=  rx .  ~2+rz .  ~2 ; 

'/range  squared 

zetal_dot 

=  1/CONST . dt uneas/CONST . R  * 

(rz./r2).~2;  °/0Deriv  of  FIM  elements 

zeta2_dot 

=  1/CONST. dtjneas/CONST.R  * 

(rx. /r2) . "2 ; 

zeta3_dot 

=  1/CONST. dtuneas/CONST.R  * 

- (rx . *rz) . / (r2 . "2) ; 

Xdot  =  [xdot  zdot  xddot  zddot  zetal_dot  zeta2_dot  zeta3_dot] ; 

path  =  atan2(rz,rx) ; 

Xdot  .path  =  [Xdot  path]  ; 

’/,’/,  Calculate  analytic  derivatives 

if  (WHAT_T0_RUN==1  &&  nargout==2)  I  I  (WHAT.T0 _RUN==2  Sck  nargout==3)  .  .  . 

II  (WHAT_T0_RUN==3  &&  nargout==3)  7,if  analytic  deriv’s  are  used 

[N  n]  =size  (X)  ; 

DerivDAE=zeros ( (n+1) *N,  10);  7,init .  dimensions:  N(n+c)  x  (n+m+q+1) 
"/,(N=nodes,  n=states,  m=controls,  q=parameters ,  c=paths) 

y.7.fl:  dX_l/dt  =  xdot  f  is  the  derivatives  of  the  states 

7,df  1=  [df l.dx  dfl.dz  dfl.dxdot  dfl.dzdot  dfl.dzetal  dfl_dzeta2  ... 

/  df  l_dzeta3  dfl.dul  dfl_du2  df  l.dt]  ; 

dfl.dxdot  =  ones(N,l);  7,Calculate  the  non-zero  partials 

DerivDAE(l :  N,3)=  dfl.dxdot;  ’/.Update  the  elements  that  are  non-zero 


’/.’/ f2:  dX_2/dt  =  zdot 

’/.df 2=  [df 2_dx  df2_dz  df2_dxdot  df2_dzdot  df2_dzetal  df2_dzeta2  ... 

"/,  df  2_dzeta3  df  2_dul  df  2_du2  df  2_dt]  ; 

df2_dzdot  =  ones(N,l);  "/.Calculate  the  non-zero  partials 

DerivDAE(N+l :  2*N,4)  =  df2_dzdot;  "/.Update  the  elements  that  are  non-zero 


y,"/.f 3 :  dX_3/dt=xddot 

"/.df 3=  [df 3_dx  df3_dz  df3_dxdot  df3_dzdot  df3_dzetal  df3_dzeta2  ... 

"/,  df 3_dzeta3  df  3_dul  df  3_du2  df  3_dt]  ; 

df3_dul  =  ones(N,l);  ’/.Calculate  the  non-zero  partials 

DerivDAE(2*N+l :  3*N,8)  =  df3_dul;  ’/.Update  the  elements  that  are  non-zero 

’/,’/,f4:  dX_4/dt=zddot 

’/,  df4  =  [df4_dx  df4_dz  df4_dxdot  df4_dzdot  df4_dzetal  df4_dzeta2  .  .  . 

’/,  df4_dzeta3  df4_dul  df4_du2  df4_dt]  ; 

df4_du2  =  ones(N,l);  '/.Calculate  the  non-zero  partials 

DerivDAE(3*N+l  :4*N,9)  =  df4_du2;  "/.update  the  non-zero  elements 


’/,’/,f5:  dX_5/dt=l/dt jneas/R  *  rz~2/ (rx“2+rz~2) “2 

’/,  df5  =  [df5_dx  df5_dz  df5_dxdot  df5_dzdot  df5_dzetal  df5_dzeta2  .  .  . 
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"/,  df  5_dzeta3  df  5_dul  df 5_du2  df  5_dt]  ; 

"/.Calculate  the  non-zero  partials 

df  5_dx  =  4/CONST.  dt_meas/CONST.  R*rz .  “2 .  *rx .  /r2 .  ~3 ; 

df5_dz  =  -2/CONST .  dt_meas/CONST  .  R*rz .  *  (rx+rz)  .  *  (rx-rz)  .  /r2 .  “3 ; 

DerivDAE(4*N+l : 5*N, 1 : 2)= [df 5_dx  df5_dz]  ;  "/.update  the  non-zero  elements 

"/,’/,f6:  dX_6/dt=l/dt_meas/R  *  rx~2/  (rx“2+rz~2)  ~2 

"/.  df6=[df6_dx  df6_dz  df6_dxdot  df6_dzdot  df6_dzetal  df6_dzeta2  ... 

"/,  df6_dzeta3  df6_dul  df6_du2  df6_dt]  ; 

"/.Calculate  the  non-zero  partials 

df6_dx  =  2/CONST.  dt_meas/CONST.  R*rx.  *  (rx+rz)  .*  (rx-rz)  .  /r2 .  “3 ; 

df  6_dz  =  4/CONST.  dt_meas/C0NST.R*rx.~2.*rz./r2.~3; 

DerivDAE(5*N+l : 6*N, 1 : 2)  =  [df  6_dx  df6_dz]  ;  "/.update  the  non-zero  elements 


"/,'/,f  7 :  dX_7/dt=l/dt_meas/R  *  -rx*rz/ (rx~2+rz~2)  ~2 

’/,  df  7  =  [df7_dx  df  7_dz  df7_dxdot  df  7_dzdot  df7_dzetal  df7_dzeta2  .  .  . 

"/,  df  7_dzeta3  df7_dul  df7_du2  df  7_dt]  ; 

"/.Calculate  the  non-zero  partials 

df 7_dx  =  1 /CONST. dtuneas /CONST. R*rz . * (rz . ~2-3*rx . “2) . /r2 . "3; 

df  7_dz  =  1 /CONST.  dt_meas /CONST.  R*rx .  *  (rx .  ~2-3*rz.  ~2)  .  /r2 .  "3; 

DerivDAE(6*N+l :  7*N,  1 :  2)  =  [df  7_dx  df7_dz]  ;  "/.update  the  non-zero  elements 


"/,  Path  Constraint  Cl:  atan2(rz,rx) 

"/,  del  =  [dcl_dx  dcl_dz  dcl.dxdot  dcl_dzdot  dcl/dzetal  dcl/dzeta2  .  .  . 

"/,  dcl/dzeta3  dcl_dul  dcl_du2  dcl_dt]  ; 

’/.Calculate  the  non-zero  partials 
dcl.dx  =  rz./r2; 

dcl_dz  =-rx./r2; 

DerivDAE(7*N+l :  8*N,  1 :  2)  =  [dcl_dx  dcl.dz]  ;  "/.update  the  non-zero  elements 

end 

if  WHAT_T0_RUN==1  "/.Format  for  GPOPS  2.4 
output  1  =Xdot  _path ; 
output3=  []  ; 
if  nargout==2 

output2=Der  ivDAE ; 

else 

output2=[]  ; 

end 

elseif  WHAT_T0_RUN==2  I  I  WHAT_T0 _RUN==3  "/.Format  for  GPOPS  3.2  &  GPOPS  3.3 
outputl=Xdot ; 
output2=path; 
if  nargout==3 

output3=Der  ivDAE ; 

else 

output3=  []  ; 

end 

end 


Trajectory  Planner  Event  Function. 


function  [events  Derivevents]  =traj ectory_planner_event  (solevents) 

%  This  function  provides  the  evaluation  of  the  event  constraint 
*/.  (boundary  condition  on  a  combination  of  states) ,  and  the  analytic 
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0005  7.  partial  derivatives  about  the  constraint.  The  event  constraint  used  is 
0006  /  positive  when  the  required  final  covariance  in  the  associated  axis  is 
0007  7,  expected  to  be  met . 

0008 

0009  global  CONST  WHAT_T0_RUN 
0010 

0011  Xf  =  solevents. terminal. state; 

0012  7.  t0  =  solevents.  intial. time; 

0013  7.  X0  =  solevents .  intial .  state ; 

0014  7o  tf  =  solevents. terminal. time; 

0015  7o  p  =  solevents  .parameter ; 

0016  7o  iphase=solevents .phase ; 

0017 

0018  zetal_tf  =  Xf(5); 

0019  zeta2_tf  =  Xf(6); 

0020  zeta3_tf  =  Xf(7); 

0021 

0022  deni  =  (zetal_tf  *zeta2_tf-zeta3_tf  '‘2) ;  7.Calculate  a  common  denominator  once 
0023 

0024  event 1  =  CONST. Pxx_f  *  deni  -  zeta2_tf ; 

0025  event2  =  CONST. Pzz_f  *  deni  -  zetal.tf ; 

0026 

0027  events  =  [eventl;  event2] ; 

0028 

0029  if  nargout==2  7.Calculate  analytic  partial  derivatives 
0030 

0031  7.7.  NOTE:  The  order  of  the  derivatives  has  changed.  The  old  order  for 

0032  7.7.  GP0PS  2 .  ~  is  Derivevents=  [dE/dX(t0)  dE/dtO  dE/dX(tf)  dE/dtf  dE/dp] 

0033  7.7.  and  is  reflected  in  the  body  below.  The  order  is  changed  at  the 

0034  7.7.  bottom  for  GP0PS  3.~ 

0035 

0036  Derivevents=zeros(2, 16) ;  7.init .  size=  (e,  2n+2+q) 

0037 

0038  7.7.El=Pxx_f  (zetal_f  *zeta2_f-zeta3_f  ~2)  -zeta2_f 

0039  7.  dEl=  [dEl_dx0  dEl.dzO  dEl.dxdotO  dEl_dzdot0  dEl_dzetal_0  ... 

0040  7.  dEl_dzeta2_0  dEl_dzeta3_0  dEl.dtO  dEl_dxf  dEl_dzf  .  .  . 

0041  7.  dEl_dxdotf  dEl_dzdotf  dEl_dzetal_f  dEl_dzeta2_f  dEl_dzeta3_f  .  .  . 

0042  7.  dEl.dtf  dEl.dp]  ; 

0043 

0044  7.Calculate  non-zero  partial  derivatives 

0045  dEl_dzetal_f =C0NST .  Pxx_f  *zeta2_tf  ; 

0046  dEl_dzeta2_f=C0NST.Pxx_f  *zetal_tf-l ; 

0047  dEl_dzeta3_f=-2*C0NST.Pxx_f  *zeta3_tf ; 

0048 

0049  7.7.E2=Pzz_f  (zetal_f  *zeta2_f-zeta3_f  ~2)  -zetal.f 

0050  7.  dE2=  [dE2_dxO  dE2_dzO  dE2_dxdotO  dE2_dzdotO  dE2_dzetal_0  ... 

0051  7.  dE2_dzeta2_0  dE2_dzeta3_0  dE2_dtO  dE2_dxf  dE2_dzf  .  .  . 

0052  7.  dE2_dxdotf  dE2_dzdotf  dE2_dzetal_f  dE2_dzeta2_f  .  .  . 

0053  7.  dE2_dzeta3_f  dE2_dtf  dE2_dp]  ; 

0054 

0055  7.Calculate  non-zero  partial  derivatives 

0056  dE2_dzetal_f =C0NST .  Pzz_f  *zeta2_tf-l ; 

0057  dE2_dzeta2_f=C0NST.Pzz_f  *zetal_tf ; 

0058  dE2_dzeta3_f =-2*C0NST .  Pzz_f  *zeta3_tf  ; 

0059 

0060  7.update  non-zero  elements  (note — the  order  of  the  derivatives  has 

0061  7.changed  between  GP0PS  2.~  series  and  GP0PS  3."  series). 

0062  if  WHAT_T0_RUN==1  7.Format  for  GP0PS  2.~ 

0063  DeriveventsC  :  , 13 : 15)= [dEl_dzetal_f  dEl_dzeta2_f  dEl_dzeta3_f  ;  .  .  . 

0064  dE2_dzetal_f  dE2_dzeta2_f  dE2_dzeta3_f ]  ; 

0065  elseif  WHAT_T0_RUN==2  I  I  WHAT_T0_RUN==3  7,Format  for  GP0PS  3.~ 

0066  DeriveventsC 10)  =  [dEl_dzetal_f  ;  dE2_dzetal_f]  ; 

0067  DeriveventsC  12)  =  [dEl_dzeta2_f  ;  dE2_dzeta2_f]  ; 

0068  DeriveventsC  14)  =  [dEl_dzeta3_f  ;  dE2_dzeta3_f]  ; 

0069  end 

0070  else 


205 


0071  Derivevents=  [] ; 
0072  end 


206 
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